diff --git a/doc/ros_api.md b/doc/ros_api.md index 0f4950b7..47ed915d 100644 --- a/doc/ros_api.md +++ b/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) diff --git a/lib/MotoROS_PlatformLib.dnLib b/lib/MotoROS_PlatformLib.dnLib index 12e055e9..108ff7a2 100644 Binary files a/lib/MotoROS_PlatformLib.dnLib and b/lib/MotoROS_PlatformLib.dnLib differ diff --git a/lib/MotoROS_PlatformLib.h b/lib/MotoROS_PlatformLib.h index ae88761a..5c0f9af3 100644 --- a/lib/MotoROS_PlatformLib.h +++ b/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 diff --git a/lib/MotoROS_PlatformLib.yrcLib b/lib/MotoROS_PlatformLib.yrcLib index 0d57a111..96bfaa00 100644 Binary files a/lib/MotoROS_PlatformLib.yrcLib and b/lib/MotoROS_PlatformLib.yrcLib differ diff --git a/lib/MotoROS_PlatformLib.yrcmLib b/lib/MotoROS_PlatformLib.yrcmLib index e64e36c5..1d42780b 100644 Binary files a/lib/MotoROS_PlatformLib.yrcmLib and b/lib/MotoROS_PlatformLib.yrcmLib differ diff --git a/src/CommunicationExecutor.c b/src/CommunicationExecutor.c index f46c7974..d7500fe5 100644 --- a/src/CommunicationExecutor.c +++ b/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 diff --git a/src/CommunicationExecutor.h b/src/CommunicationExecutor.h index f4112fa0..79f72a7e 100644 --- a/src/CommunicationExecutor.h +++ b/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 { diff --git a/src/ErrorHandling.h b/src/ErrorHandling.h index b1ae1325..9398a38d 100644 --- a/src/ErrorHandling.h +++ b/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 diff --git a/src/MotoROS.h b/src/MotoROS.h index 3f2f5a88..0e3a46e4 100644 --- a/src/MotoROS.h +++ b/src/MotoROS.h @@ -70,6 +70,8 @@ #include #include #include +#include +#include //============================================ // 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" diff --git a/src/MotoROS2_AllControllers.vcxproj b/src/MotoROS2_AllControllers.vcxproj index b5fad929..a7064d4b 100644 --- a/src/MotoROS2_AllControllers.vcxproj +++ b/src/MotoROS2_AllControllers.vcxproj @@ -343,6 +343,7 @@ + @@ -377,6 +378,7 @@ + diff --git a/src/MotoROS2_AllControllers.vcxproj.filters b/src/MotoROS2_AllControllers.vcxproj.filters index 6b585860..77f66e90 100644 --- a/src/MotoROS2_AllControllers.vcxproj.filters +++ b/src/MotoROS2_AllControllers.vcxproj.filters @@ -327,6 +327,9 @@ Source Files\Services + + Source Files\Services + Source Files\Robot Controller @@ -440,6 +443,9 @@ Header Files\Services + + Header Files\Services + Header Files\Robot Controller diff --git a/src/RosApiNameConstants.h b/src/RosApiNameConstants.h index 4523d31a..798f0415 100644 --- a/src/RosApiNameConstants.h +++ b/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" diff --git a/src/ServiceGetActiveAlarmInfo.c b/src/ServiceGetActiveAlarmInfo.c new file mode 100644 index 00000000..5fba180d --- /dev/null +++ b/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); +} diff --git a/src/ServiceGetActiveAlarmInfo.h b/src/ServiceGetActiveAlarmInfo.h new file mode 100644 index 00000000..415d65ba --- /dev/null +++ b/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 diff --git a/src/main.c b/src/main.c index 5857270f..aebf21f4 100644 --- a/src/main.c +++ b/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();