New service: allow retrieving information about active alarms and/or errors

doc/ros_api.md

@@ -73,6 +73,14 @@ Note: this topic is only namespaced if a namespace is configured *and* `namespac
 
 ## Services
 
+### get_active_alarm_info
+
+Type: [motoros2_interfaces/srv/GetActiveAlarmInfo](https://github.com/Yaskawa-Global/motoros2_interfaces/blob/3ec808b361e103d4fd8c63a964df5bd593c923be/srv/GetActiveAlarmInfo.srv)
+
+Retrieve information about active alarms and/or errors on the controller.
+
+Please refer to the documentation embedded in the service definition for more information about addressing and general service behaviour.
+
 ### read_group_io
 
 Type: [motoros2_interfaces/srv/ReadGroupIO](https://github.com/yaskawa-global/motoros2_interfaces/blob/d6805d32714df4430f7db3d8ddc736c340ddeba8/srv/ReadGroupIO.srv)

lib/MotoROS_PlatformLib.h

@@ -18,7 +18,7 @@
 // -  ZZ: patch version nr
 //
 // Note: no leading zeros, to avoid octal parsing
-#define MOTOROS_PLATFORM_LIB_VERSION 212
+#define MOTOROS_PLATFORM_LIB_VERSION 213
 
 #define MOTOROS_PLATFORM_LIB_MAJOR (MOTOROS_PLATFORM_LIB_VERSION / 100000)
 #define MOTOROS_PLATFORM_LIB_MINOR (MOTOROS_PLATFORM_LIB_VERSION / 100 % 1000)
@@ -105,4 +105,37 @@ extern int clock_gettime(clockid_t clock_id, /* clock ID (always CLOCK_REALTIME)
 extern STATUS Ros_UserLan_IsLinkUp(USHORT if_no, BOOL* const is_up /*out*/);
 
 
+// Retrieve information associated with the alarm message at 'usIndex'.
+// If an index (ie: slot) does not have an active alarm, ERROR is returned.
+#define ROS_MAX_ALARM_MSG_LEN (36)
+typedef struct
+{
+    USHORT usIndex;
+    CHAR reserved[6];
+} ROS_ALARM_INFO_SEND_DATA;
+typedef struct
+{
+    LONG code;
+    LONG subcode;
+    CHAR msg[ROS_MAX_ALARM_MSG_LEN + 1];
+    CHAR reserved[1];
+} ROS_ALARM_INFO_RSP_DATA ;
+
+extern STATUS Ros_GetAlarmInfo(ROS_ALARM_INFO_SEND_DATA const* sData, ROS_ALARM_INFO_RSP_DATA* const rData);
+
+
+// Retrieve information associated with the currently active error.
+// If there is no active error, ERROR is returned.
+#define ROS_MAX_ERROR_MSG_LEN (36)
+typedef struct
+{
+    LONG code;
+    LONG subcode;
+    CHAR msg[ROS_MAX_ERROR_MSG_LEN + 1];
+    CHAR reserved[1];
+} ROS_ERROR_INFO_RSP_DATA;
+
+extern STATUS Ros_GetErrorInfo(ROS_ERROR_INFO_RSP_DATA* const rData);
+
+
 #endif  // MOTOROS_PLATFORM_LIB_H

src/CommunicationExecutor.c

@@ -429,6 +429,11 @@ void Ros_Communication_StartExecutors(SEM_ID semCommunicationExecutorStatus)
         &g_messages_ReadWriteIO.resp_mreg_write, Ros_ServiceWriteMRegister_Trigger);
     motoRosAssert_withMsg(rc == RCL_RET_OK, SUBCODE_FAIL_ADD_SERVICE_WRITE_M_REG, "Failed adding service (%d)", (int)rc);
 
+    rc = rclc_executor_add_service(
+        &executor_io_control, &g_serviceGetActiveAlarmInfo, &g_messages_GetActiveAlarmInfo.request,
+        &g_messages_GetActiveAlarmInfo.response, Ros_ServiceGetActiveAlarmInfo_Trigger);
+    motoRosAssert_withMsg(rc == RCL_RET_OK, SUBCODE_FAIL_ADD_SERVICE_GET_ACTIVE_ALARM_INFO, "Failed adding service (%d)", (int)rc);
+
     //===========================================================
 
     // Optional prepare for avoiding allocations during spin

src/CommunicationExecutor.h

@@ -26,7 +26,8 @@
 // total number of handles =
 //      timers +                                            1
 //      service read & write I/O +                          6
-#define QUANTITY_OF_HANDLES_FOR_IO_EXECUTOR                 (7)
+//      service get active alarm info                       1
+#define QUANTITY_OF_HANDLES_FOR_IO_EXECUTOR                 (8)
 
 typedef struct
 {

src/ErrorHandling.h

@@ -169,6 +169,8 @@ typedef enum
     SUBCODE_CONFIGURATION_FAIL_MP_NICDATA1,
     SUBCODE_FAIL_MP_NICDATA_INIT1,
     SUBCODE_FAIL_INVALID_BASE_TRACK_MOTION_TYPE,
+    SUBCODE_FAIL_ADD_SERVICE_GET_ACTIVE_ALARM_INFO,
+    SUBCODE_FAIL_INIT_SERVICE_GET_ACTIVE_ALARM_INFO,
 
 } ALARM_ASSERTION_FAIL_SUBCODE; //8011
 

src/MotoROS.h

@@ -70,6 +70,8 @@
 #include <motoros2_interfaces/srv/start_point_queue_mode.h>
 #include <motoros2_interfaces/srv/queue_traj_point.h>
 #include <motoros2_interfaces/srv/select_motion_tool.h>
+#include <motoros2_interfaces/srv/get_active_alarm_info.h>
+#include <motoros2_interfaces/msg/alarm_info.h>
 
 //============================================
 // MotoROS
@@ -94,6 +96,7 @@
 #include "ServiceStartPointQueueMode.h"
 #include "ServiceStopTrajMode.h"
 #include "ServiceSelectMotionTool.h"
+#include "ServiceGetActiveAlarmInfo.h"
 #include "MotionControl.h"
 #include "ConfigFile.h"
 #include "RosApiNameConstants.h"

src/MotoROS2_AllControllers.vcxproj

@@ -343,6 +343,7 @@
     <ClCompile Include="ServiceStopTrajMode.c" />
     <ClCompile Include="ServiceStartTrajMode.c" />
     <ClCompile Include="ServiceSelectMotionTool.c" />
+    <ClCompile Include="ServiceGetActiveAlarmInfo.c" />
     <ClCompile Include="Tests_ActionServer_FJT.c" />
     <ClCompile Include="Tests_ControllerStatusIO.c" />
     <ClCompile Include="Tests_CtrlGroup.c" />
@@ -377,6 +378,7 @@
     <ClInclude Include="ServiceStopTrajMode.h" />
     <ClInclude Include="ServiceStartTrajMode.h" />
     <ClInclude Include="ServiceSelectMotionTool.h" />
+    <ClInclude Include="ServiceGetActiveAlarmInfo.h" />
     <ClInclude Include="Tests_ActionServer_FJT.h" />
     <ClInclude Include="Tests_ControllerStatusIO.h" />
     <ClInclude Include="Tests_CtrlGroup.h" />

src/MotoROS2_AllControllers.vcxproj.filters

@@ -327,6 +327,9 @@
     <ClCompile Include="ServiceStartPointQueueMode.c">
       <Filter>Source Files\Services</Filter>
     </ClCompile>
+    <ClCompile Include="ServiceGetActiveAlarmInfo.c">
+      <Filter>Source Files\Services</Filter>
+    </ClCompile>
     <ClCompile Include="Ros_mpGetRobotCalibrationData.c">
       <Filter>Source Files\Robot Controller</Filter>
     </ClCompile>
@@ -440,6 +443,9 @@
     <ClInclude Include="ServiceStartPointQueueMode.h">
       <Filter>Header Files\Services</Filter>
     </ClInclude>
+    <ClInclude Include="ServiceGetActiveAlarmInfo.h">
+      <Filter>Header Files\Services</Filter>
+    </ClInclude>
     <ClInclude Include="Ros_mpGetRobotCalibrationData.h">
       <Filter>Header Files\Robot Controller</Filter>
     </ClInclude>

src/RosApiNameConstants.h

@@ -28,6 +28,7 @@
 #define SERVICE_NAME_STOP_TRAJ_MODE "stop_traj_mode"
 #define SERVICE_NAME_QUEUE_TRAJ_POINT "queue_traj_point"
 #define SERVICE_NAME_SELECT_MOTION_TOOL "select_motion_tool"
+#define SERVICE_NAME_GET_ACTIVE_ALARM_INFO "get_active_alarm_info"
 
 #define ACTION_NAME_FOLLOW_JOINT_TRAJECTORY "follow_joint_trajectory"
 

src/ServiceGetActiveAlarmInfo.c

@@ -0,0 +1,194 @@
+//ServiceGetActiveAlarmInfo.c
+
+// SPDX-FileCopyrightText: 2023, Yaskawa America, Inc.
+// SPDX-FileCopyrightText: 2023, Delft University of Technology
+//
+// SPDX-License-Identifier: Apache-2.0
+
+#include "MotoROS.h"
+
+rcl_service_t g_serviceGetActiveAlarmInfo;
+
+ServiceGetActiveAlarmInfo_Messages g_messages_GetActiveAlarmInfo;
+
+typedef motoros2_interfaces__srv__GetActiveAlarmInfo_Response GetActiveAlarmInfoResponse;
+
+
+static micro_ros_utilities_memory_rule_t mem_rules_response_[] =
+{
+    {"result_message", 50},
+
+    {"alarms", MAX_ALARM_COUNT},
+    {"alarms.message", ROS_MAX_ALARM_MSG_LEN},
+    //field reserved for future use, zero-allocation
+    //NOTE: zero-length strings don't appear to work: init length 1
+    {"alarms.contents", 1},
+    //field reserved for future use, zero-allocation
+    //NOTE: zero-length strings don't appear to work: init length 1
+    {"alarms.description", 1},
+    //field reserved for future use, zero-allocation
+    {"alarms.help", 0},
+
+    {"errors", MAX_ERROR_COUNT},
+    {"errors.message", ROS_MAX_ERROR_MSG_LEN},
+};
+static micro_ros_utilities_memory_conf_t mem_conf_response_ = { 0 };
+static const rosidl_message_type_support_t* type_support_response_ = NULL;
+
+
+void Ros_ServiceGetActiveAlarmInfo_Initialize()
+{
+    MOTOROS2_MEM_TRACE_START(svc_get_active_alarm_info_init);
+
+    // init request: empty, so nothing more to do after this
+    motoRosAssert_withMsg(
+        motoros2_interfaces__srv__GetActiveAlarmInfo_Request__init(
+            &g_messages_GetActiveAlarmInfo.request),
+        SUBCODE_FAIL_INIT_SERVICE_GET_ACTIVE_ALARM_INFO,
+        "Failed to init request");
+
+    // init response
+    mem_conf_response_.allocator = &g_motoros2_Allocator;
+    mem_conf_response_.rules = mem_rules_response_;
+    mem_conf_response_.n_rules = sizeof(mem_rules_response_) / sizeof(mem_rules_response_[0]);
+    type_support_response_ = ROSIDL_GET_MSG_TYPE_SUPPORT(
+        motoros2_interfaces, srv, GetActiveAlarmInfo_Response);
+
+    motoRosAssert_withMsg(
+        micro_ros_utilities_create_message_memory(
+            type_support_response_, &g_messages_GetActiveAlarmInfo.response, mem_conf_response_),
+        SUBCODE_FAIL_INIT_SERVICE_GET_ACTIVE_ALARM_INFO,
+        "Failed to init response");
+
+    g_messages_GetActiveAlarmInfo.response.alarms.size = 0;
+    g_messages_GetActiveAlarmInfo.response.errors.size = 0;
+
+    rcl_ret_t ret = rclc_service_init_default(
+        &g_serviceGetActiveAlarmInfo, &g_microRosNodeInfo.node,
+        ROSIDL_GET_SRV_TYPE_SUPPORT(motoros2_interfaces, srv, GetActiveAlarmInfo),
+        SERVICE_NAME_GET_ACTIVE_ALARM_INFO);
+    motoRosAssert_withMsg(ret == RCL_RET_OK,
+        SUBCODE_FAIL_INIT_SERVICE_GET_ACTIVE_ALARM_INFO, "Failed to init service (%d)",
+        (int)ret);
+
+    MOTOROS2_MEM_TRACE_REPORT(svc_get_active_alarm_info_init);
+}
+
+void Ros_ServiceGetActiveAlarmInfo_Cleanup()
+{
+    MOTOROS2_MEM_TRACE_START(svc_get_active_alarm_info_fini);
+
+    Ros_Debug_BroadcastMsg("Cleanup service " SERVICE_NAME_GET_ACTIVE_ALARM_INFO);
+    rcl_ret_t ret = rcl_service_fini(
+        &g_serviceGetActiveAlarmInfo, &g_microRosNodeInfo.node);
+    if (ret != RCL_RET_OK)
+        Ros_Debug_BroadcastMsg("Failed cleaning up "
+            SERVICE_NAME_GET_ACTIVE_ALARM_INFO " service: %d", ret);
+
+    //use destroy_message_memory(..) here as we initialised using
+    //create_message_memory(..) and it knows how to process the memory
+    //configuration struct
+    bool result = micro_ros_utilities_destroy_message_memory(
+        type_support_response_, &g_messages_GetActiveAlarmInfo.response, mem_conf_response_);
+    Ros_Debug_BroadcastMsg("%s: cleanup response msg memory: %d", __func__, result);
+
+    MOTOROS2_MEM_TRACE_REPORT(svc_get_active_alarm_info_fini);
+}
+
+void Ros_ServiceGetActiveAlarmInfo_Trigger(const void* request_msg, void* response_msg)
+{
+    RCL_UNUSED(request_msg);
+    GetActiveAlarmInfoResponse* response = (GetActiveAlarmInfoResponse*)response_msg;
+
+    Ros_Debug_BroadcastMsg("%s: retrieving active alarm & error info", __func__);
+
+    // retrieve active alarms and/or errors
+    MP_ALARM_CODE_RSP_DATA alarmData;
+    bzero(&alarmData, sizeof(alarmData));
+    if (mpGetAlarmCode(&alarmData) != OK)
+    {
+        // can't continue if M+ API fails
+        Ros_Debug_BroadcastMsg("%s: error retrieving alarm data", __func__);
+        // TODO: use proper error codes + strings
+        rosidl_runtime_c__String__assign(&response->result_message, "M+ API error");
+        response->result_code = -1;
+        goto DONE;
+    }
+
+    // NOTE: we exploit the fact a bool is an int in C and avoid having to use
+    // a full if-else to print the nr of active errors that way (last arg to
+    // Ros_Debug_BroadcastMsg(..))
+    Ros_Debug_BroadcastMsg("%s: %d alarm(s), %d error(s)", __func__,
+        alarmData.usAlarmNum, 0 != alarmData.usErrorNo);
+
+    // assume there are no alarms, nor errors
+    response->alarms.size = 0;
+    response->errors.size = 0;
+
+    // retrieve info on active alarms and copy data
+    response->alarms.size = alarmData.usAlarmNum;
+    for(size_t i = 0; i < response->alarms.size; ++i)
+    {
+        Ros_Debug_BroadcastMsg("%s: processing alarm %04d (%d out of %d)",
+            __func__, alarmData.AlarmData.usAlarmNo[i],
+            i + 1, response->alarms.size);
+
+        response->alarms.data[i].number = alarmData.AlarmData.usAlarmNo[i];
+        response->alarms.data[i].sub_code = alarmData.AlarmData.usAlarmData[i];
+
+        // retrieve info on active alarm
+        ROS_ALARM_INFO_SEND_DATA alarmInfoSendData = { 0 };
+        ROS_ALARM_INFO_RSP_DATA alarmInfoData;
+        alarmInfoSendData.usIndex = i;
+        bzero(&alarmInfoData, sizeof(ROS_ALARM_INFO_RSP_DATA));
+        STATUS status = Ros_GetAlarmInfo(&alarmInfoSendData, &alarmInfoData);
+        if (status != OK)
+        {
+            Ros_Debug_BroadcastMsg(
+                "%s: error retrieving alarm info (%d), skipping", __func__, status);
+        }
+        else
+        {
+            rosidl_runtime_c__String__assignn(
+                &response->alarms.data[i].message, alarmInfoData.msg, ROS_MAX_ALARM_MSG_LEN);
+        }
+    }
+
+    // retrieve info on active error(s)
+    if (0 < alarmData.usErrorNo)
+    {
+        // there can be only a single active error at any given time, but use
+        // the same code structure as for warnings (but just use constants)
+        const size_t kNumActiveErrors = 1;
+        const size_t kFirstErrorIdx = 0;
+        Ros_Debug_BroadcastMsg("%s: processing error %04d (%d out of %d)",
+            __func__, alarmData.usErrorNo, kNumActiveErrors, MAX_ERROR_COUNT);
+
+        response->errors.size = kNumActiveErrors;
+        response->errors.data[kFirstErrorIdx].number = alarmData.usErrorNo;
+        response->errors.data[kFirstErrorIdx].sub_code = alarmData.usErrorData;
+
+        // retrieve info on active error
+        ROS_ERROR_INFO_RSP_DATA errorInfoData;
+        bzero(&errorInfoData, sizeof(ROS_ERROR_INFO_RSP_DATA));
+        STATUS status = Ros_GetErrorInfo(&errorInfoData);
+        if (status != OK)
+        {
+            Ros_Debug_BroadcastMsg(
+                "%s: error retrieving error info (%d), skipping", __func__, status);
+        }
+        else
+        {
+            rosidl_runtime_c__String__assignn(
+                &response->errors.data[kFirstErrorIdx].message, errorInfoData.msg, ROS_MAX_ERROR_MSG_LEN);
+        }
+    }
+
+    // TODO: use proper error codes + strings
+    rosidl_runtime_c__String__assign(&response->result_message, "success");
+    response->result_code = 1;
+
+DONE:
+    Ros_Debug_BroadcastMsg("%s: exit: '%s' (%lu)", __func__, response->result_message.data,
+        (unsigned long) response->result_code);
+}

src/ServiceGetActiveAlarmInfo.h

@@ -0,0 +1,25 @@
+//ServiceGetActiveAlarmInfo.h
+
+// SPDX-FileCopyrightText: 2023, Yaskawa America, Inc.
+// SPDX-FileCopyrightText: 2023, Delft University of Technology
+//
+// SPDX-License-Identifier: Apache-2.0
+
+#ifndef MOTOROS2_SERVICE_GET_ACTIVE_ALARM_INFO_H
+#define MOTOROS2_SERVICE_GET_ACTIVE_ALARM_INFO_H
+
+extern rcl_service_t g_serviceGetActiveAlarmInfo;
+
+typedef struct
+{
+    motoros2_interfaces__srv__GetActiveAlarmInfo_Request request;
+    motoros2_interfaces__srv__GetActiveAlarmInfo_Response response;
+} ServiceGetActiveAlarmInfo_Messages;
+extern ServiceGetActiveAlarmInfo_Messages g_messages_GetActiveAlarmInfo;
+
+extern void Ros_ServiceGetActiveAlarmInfo_Initialize();
+extern void Ros_ServiceGetActiveAlarmInfo_Cleanup();
+
+extern void Ros_ServiceGetActiveAlarmInfo_Trigger(const void* request_msg, void* response_msg);
+
+#endif  // MOTOROS2_SERVICE_GET_ACTIVE_ALARM_INFO_H

src/main.c

@@ -130,6 +130,7 @@ void RosInitTask()
         Ros_ServiceStartPointQueueMode_Initialize();
         Ros_ServiceStopTrajMode_Initialize();
         Ros_ServiceSelectMotionTool_Initialize();
+        Ros_ServiceGetActiveAlarmInfo_Initialize();
 
         // Start executor that performs all communication
         // (This task deletes itself when the agent disconnects.)
@@ -202,6 +203,7 @@ void RosInitTask()
         mpSemTake(semCommunicationExecutorStatus, WAIT_FOREVER);
         mpSemDelete(semCommunicationExecutorStatus);
 
+        Ros_ServiceGetActiveAlarmInfo_Cleanup();
         Ros_ServiceSelectMotionTool_Cleanup();
         Ros_ServiceStopTrajMode_Cleanup();
         Ros_ServiceStartTrajMode_Cleanup();