diff --git a/.github/workflows/ci_md_lint.yaml b/.github/workflows/ci_md_lint.yaml index bd240c73..906ac237 100644 --- a/.github/workflows/ci_md_lint.yaml +++ b/.github/workflows/ci_md_lint.yaml @@ -12,6 +12,6 @@ jobs: runs-on: ubuntu-latest steps: - uses: actions/checkout@v4 - - uses: DavidAnson/markdownlint-cli2-action@v16 + - uses: DavidAnson/markdownlint-cli2-action@v17 with: globs: "**/*.md" diff --git a/.github/workflows/ci_msbuild.yaml b/.github/workflows/ci_msbuild.yaml index d964b7d8..0a448fe0 100644 --- a/.github/workflows/ci_msbuild.yaml +++ b/.github/workflows/ci_msbuild.yaml @@ -60,9 +60,9 @@ jobs: # officially matrix doesn't support non-scalar values, but it does # seem to work, so let's use it. uros: [ - { release: 20240710, codename: foxy }, - { release: 20240710, codename: galactic }, - { release: 20240710, codename: humble }, + { release: 20240917, codename: foxy }, + { release: 20240917, codename: galactic }, + { release: 20240917, codename: humble }, ] steps: diff --git a/.gitignore b/.gitignore index dc798d5e..9c7c9646 100644 --- a/.gitignore +++ b/.gitignore @@ -364,3 +364,4 @@ libmicroros_yrc1000u_iron/ # M+ build output *.out +/libmicroros_yrc1000_iron diff --git a/README.md b/README.md index d0413100..4af9c8dd 100644 --- a/README.md +++ b/README.md @@ -98,11 +98,10 @@ The following general requirements must be met in order to be able to use MotoRO - `YAS2.80.00-00` for YRC1000 - `YBS2.45.00-00` for YRC1000micro - the controller must have a correctly configured network connection: - - DX200: `LAN` + - DX200 and YRC1000micro: `LAN` - YRC1000: either `LAN2` or `LAN3` - - YRC1000micro: either `LAN2` or `LAN3` -- ROS 2 version: Foxy, Galactic, Humble or Iron - MotoROS2 does not support ROS 2 Iron Irwini, nor Jazzy nor Rolling Ridley. +- ROS 2 version: Foxy, Galactic, Humble or Iron. + MotoROS2 does not support ROS 2 Jazzy nor Rolling Ridley. - Docker or a from-source build of the micro-ROS Agent - FastDDS as RMW (even when using ROS 2 Galactic) diff --git a/config/motoros2_config.yaml b/config/motoros2_config.yaml index 47e4954f..4e7773e2 100644 --- a/config/motoros2_config.yaml +++ b/config/motoros2_config.yaml @@ -104,8 +104,8 @@ sync_timeclock_with_agent: true # To disable auto-detection, uncomment 'userlan_monitor_port' below and set # it to the desired port. # -# NOTE: this setting only applies to YRC1000 and YRC1000u controllers. -# DX200 and FS100 controllers only have a single ethernet port, and any +# NOTE: this setting only applies to YRC1000 controllers. YRC1000u, DX200, +# and FS100 controllers only have a single ethernet port, and any # value other than 1 will be ignored. # # NOTE 2: auto-detection is not perfect. It can't determine whether the Agent @@ -286,3 +286,35 @@ publisher_qos: # # DEFAULT: false #ignore_missing_calib_data: false + +#----------------------------------------------------------------------------- +# Should MotoROS2 broadcast debug messages? +# +# If enabled, this will broadcast log messages on the network on port UDP 21789. +# The user can use the debug script to monitor the state of the robot, identify +# problems, and debug their code. +# +# The debug script is available under the Yaskawa-Global/motoros2 repository in +# the tools directory +# https://github.com/Yaskawa-Global/motoros2/tree/main/tools +# +# DEFAULT: true +#debug_broadcast_enabled: true + +#----------------------------------------------------------------------------- +# Which network port(s) should MotoROS2 broadcast debug messages on, if +# 'debug_broadcast_enabled' is 'true'? +# +# If not specified and 'debug_broadcast_enabled' is true, MotoROS2 will +# send messages over all network ports which are active on the controller. +# +# To choose a specific port to broadcast debug messages, uncomment +# 'debug_broadcast_port' below and set it to the desired port. +# +# NOTE 1: this setting only applies to YRC1000 controllers. YRC1000u, +# DX200, and FS100 controllers only have a single ethernet port, +# and will always default to USER_LAN1 +# +# OPTIONS: USER_LAN1, USER_LAN2 +# DEFAULT: (all available network ports) +#debug_broadcast_port: USER_LAN1 diff --git a/doc/troubleshooting.md b/doc/troubleshooting.md index eb9e1c4d..f286da9d 100644 --- a/doc/troubleshooting.md +++ b/doc/troubleshooting.md @@ -725,6 +725,21 @@ Save a copy of the output of the [debug-listener script](#debug-log-client) and Open a new issue on the [Issue tracker](https://github.com/yaskawa-global/motoros2/issues), describe the problem and attach `PANELBOX.LOG` and the debug log to the issue. Include a verbatim copy of the alarm text as seen on the teach pendant (alarm number and `[subcode]`). +### Alarm: 8011[64] + +*Example:* + +```text +ALARM 8011 + Enable LAN port 1 for debug +[64] +``` + +*Solution:* +The ETHERNET function must be enabled for the LAN interface that was specified in the config file. +Either change the interface specified in the config file to a LAN interface that is enabled, or enable the corresponding LAN interface on the controller. +Please contact your local Yaskawa representative to request the ETHERNET function if it is not enabled. + ### Alarm: 8012[xx] *Example:* @@ -932,7 +947,7 @@ ALARM 8013 The `userlan_monitor_port` key in the `motoros2_config.yaml` configuration file is set to an invalid value. LAN port monitoring will be disabled for this session. -On YRC1000 and YRC1000u, this must be set to either `USER_LAN1` or `USER_LAN2`. +On YRC1000, this must be set to either `USER_LAN1` or `USER_LAN2`. No other values are supported. @@ -958,7 +973,7 @@ To rule out a transient failure, reboot the controller. If the alarm is raised again, and if auto-detection is not needed or desired, make sure `userlan_monitor_port` is not commented out (ie: does not have a `#` at the start of the line) and set it to an appropriate value. -On YRC1000 and YRC1000u, set it to either `USER_LAN1` or `USER_LAN2`, depending on which LAN port is used to connect the controller to the PC running the micro-ROS Agent application. +On YRC1000, set it to either `USER_LAN1` or `USER_LAN2`, depending on which LAN port is used to connect the controller to the PC running the micro-ROS Agent application. If auto-detection is to be used, verify `agent_ip_address` is set to an IP that can be reached by MotoROS2 over the LAN port which is connected to the PC running the micro-ROS Agent application (either directly, or via a default gateway configured on the controller). @@ -1024,6 +1039,28 @@ In case the alarm is still raised after calibration was performed, TF broadcasti Open a new issue on the [Issue tracker](https://github.com/yaskawa-global/motoros2/issues), describe the problem and attach `PANELBOX.LOG`, `RBCALIB.DAT` and the debug log to the issue. Include a verbatim copy of the alarm text as seen on the teach pendant (alarm number and `[subcode]`). +### Alarm: 8013[17] + +*Example:* + +```text +ALARM 8013 + Bad UserLan debug port in cfg +[17] +``` + +*Solution:* +The `debug_broadcast_port` key in the `motoros2_config.yaml` configuration file is set to an invalid value. +Debug messages will be sent over all active network ports + +On YRC1000, this must be set to either `USER_LAN1` or `USER_LAN2`. + +No other values are supported. + +Example: `debug_broadcast_port: USER_LAN1`. + +After correcting the configuration, the [changes will need to be propagated to the Yaskawa controller](../README.md#updating-the-configuration). + ### Alarm: 8014[0] *Example:* diff --git a/src/ActionServer_FJT.c b/src/ActionServer_FJT.c index 544b0411..9e4afa5d 100644 --- a/src/ActionServer_FJT.c +++ b/src/ActionServer_FJT.c @@ -247,12 +247,12 @@ rcl_ret_t Ros_ActionServer_FJT_Goal_Received(rclc_action_goal_handle_t* goal_han if (!bSizeOk) { motomanErrorCode = INIT_TRAJ_TOO_BIG; - rosidl_runtime_c__String__assign(&fjt_result_response.result.error_string, - "Trajectory contains too many points (Not enough memory)."); + rosidl_runtime_c__String__assign(&fjt_result_response.result.error_string, + Ros_ErrorHandling_Init_Trajectory_Status_ToString((Init_Trajectory_Status) motomanErrorCode)); } 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)); } @@ -260,69 +260,13 @@ rcl_ret_t Ros_ActionServer_FJT_Goal_Received(rclc_action_goal_handle_t* goal_han { motomanErrorCode = INIT_TRAJ_WRONG_MODE; rosidl_runtime_c__String__assign(&fjt_result_response.result.error_string, - "Must call " SERVICE_NAME_START_TRAJ_MODE " service."); + Ros_ErrorHandling_Init_Trajectory_Status_ToString((Init_Trajectory_Status) motomanErrorCode)); } else if (!bInitOk) { motomanErrorCode = trajStatus; - switch (motomanErrorCode) - { - case INIT_TRAJ_TOO_SMALL: - rosidl_runtime_c__String__assign(&fjt_result_response.result.error_string, - "Trajectory must contain at least two points."); - break; - case INIT_TRAJ_INVALID_STARTING_POS: - rosidl_runtime_c__String__assign(&fjt_result_response.result.error_string, - "The first point must match the robot's current position."); - break; - case INIT_TRAJ_INVALID_VELOCITY: - rosidl_runtime_c__String__assign(&fjt_result_response.result.error_string, - "The commanded velocity is too high."); - break; - case INIT_TRAJ_ALREADY_IN_MOTION: - rosidl_runtime_c__String__assign(&fjt_result_response.result.error_string, - "Already running a trajectory."); - break; - case INIT_TRAJ_INVALID_JOINTNAME: - rosidl_runtime_c__String__assign(&fjt_result_response.result.error_string, - "Invalid joint name specified. Check motoros2_config.yaml."); - break; - case INIT_TRAJ_INCOMPLETE_JOINTLIST: - rosidl_runtime_c__String__assign(&fjt_result_response.result.error_string, - "Trajectory must contain data for all joints."); - break; - case INIT_TRAJ_INVALID_TIME: - rosidl_runtime_c__String__assign(&fjt_result_response.result.error_string, - "Invalid time in trajectory."); - break; - case INIT_TRAJ_BACKWARD_TIME: - rosidl_runtime_c__String__assign(&fjt_result_response.result.error_string, - "Trajectory message contains waypoints that are not strictly increasing in time."); - break; - case INIT_TRAJ_WRONG_NUMBER_OF_POSITIONS: - rosidl_runtime_c__String__assign(&fjt_result_response.result.error_string, - "Trajectory did not contain position data for all axes."); - break; - case INIT_TRAJ_WRONG_NUMBER_OF_VELOCITIES: - rosidl_runtime_c__String__assign(&fjt_result_response.result.error_string, - "Trajectory did not contain velocity data for all axes."); - break; - case INIT_TRAJ_INVALID_ENDING_VELOCITY: - rosidl_runtime_c__String__assign(&fjt_result_response.result.error_string, - "The final point in the trajectory must have zero velocity."); - break; - case INIT_TRAJ_INVALID_ENDING_ACCELERATION: - rosidl_runtime_c__String__assign(&fjt_result_response.result.error_string, - "The final point in the trajectory must have zero acceleration."); - break; - case INIT_TRAJ_DUPLICATE_JOINT_NAME: - rosidl_runtime_c__String__assign(&fjt_result_response.result.error_string, - "The trajectory contains duplicate joint names."); - break; - default: - rosidl_runtime_c__String__assign(&fjt_result_response.result.error_string, - "Trajectory initialization failed. Generic failure."); - } + rosidl_runtime_c__String__assign(&fjt_result_response.result.error_string, + Ros_ErrorHandling_Init_Trajectory_Status_ToString(trajStatus)); } fjt_result_response.result.error_code = RESULT_REPONSE_ERROR_CODE(control_msgs__action__FollowJointTrajectory_Result__INVALID_GOAL, motomanErrorCode); diff --git a/src/CommunicationExecutor.c b/src/CommunicationExecutor.c index 0d9b56d8..a955996b 100644 --- a/src/CommunicationExecutor.c +++ b/src/CommunicationExecutor.c @@ -74,7 +74,7 @@ void Ros_Communication_ConnectToAgent() "Must enable ETHERNET function"); } -#if defined (YRC1000) || defined (YRC1000u) +#if defined (YRC1000) //Try second interface if first one didn't succeed if (status != OK && (status = Ros_GetMacAddress(ROS_USER_LAN2, macId)) != OK) { diff --git a/src/ConfigFile.c b/src/ConfigFile.c index 949a6be6..13df5a64 100644 --- a/src/ConfigFile.c +++ b/src/ConfigFile.c @@ -121,6 +121,8 @@ Configuration_Item Ros_ConfigFile_Items[] = { "userlan_monitor_enabled", &g_nodeConfigSettings.userlan_monitor_enabled, Value_Bool }, { "userlan_monitor_port", &g_nodeConfigSettings.userlan_monitor_port, Value_UserLanPort }, { "ignore_missing_calib_data", &g_nodeConfigSettings.ignore_missing_calib_data, Value_Bool }, + { "debug_broadcast_enabled", &g_nodeConfigSettings.debug_broadcast_enabled, Value_Bool }, + { "debug_broadcast_port", &g_nodeConfigSettings.debug_broadcast_port, Value_UserLanPort }, }; void Ros_ConfigFile_SetAllDefaultValues() @@ -130,6 +132,10 @@ void Ros_ConfigFile_SetAllDefaultValues() //TODO(gavanderhoorn): make this an unsigned int g_nodeConfigSettings.ros_domain_id = DEFAULT_ROS_DOMAIN_ID; + //userlan debug broadcast + g_nodeConfigSettings.debug_broadcast_enabled = DEFAULT_ULAN_DEBUG_BROADCAST_ENABLED; + g_nodeConfigSettings.debug_broadcast_port = DEFAULT_ULAN_DEBUG_BROADCAST_PORT; + //========= //node_name UCHAR macId[6]; @@ -145,7 +151,7 @@ void Ros_ConfigFile_SetAllDefaultValues() "Must enable ETHERNET function"); } -#if defined (YRC1000) || defined (YRC1000u) +#if defined (YRC1000) //Try second interface if first one didn't succeed if (status != OK && (status = Ros_GetMacAddress(ROS_USER_LAN2, macId)) != OK) { @@ -325,32 +331,32 @@ void Ros_ConfigFile_CheckYamlEvent(yaml_event_t* event) break; case Value_UserLanPort: -#if defined (FS100) || defined (DX200) +#if defined (FS100) || defined (DX200) || defined (YRC1000u) // single port, override whatever was configured - *(Ros_UserLan_Port_Setting*)activeItem->valueToSet = CFG_ROS_USER_LAN1; - Ros_Debug_BroadcastMsg("DX200 or FS100: override to 'USER_LAN1'"); + * (Ros_UserLan_Port_Setting*)activeItem->valueToSet = CFG_ROS_USER_LAN1; + Ros_Debug_BroadcastMsg("DX200, YRC1000u, or FS100: override to 'USER_LAN1'"); -#elif defined (YRC1000) || defined (YRC1000u) +#elif defined (YRC1000) if (strncmp((char*)event->data.scalar.value, "USER_LAN1", 9) == 0) *(Ros_UserLan_Port_Setting*)activeItem->valueToSet = CFG_ROS_USER_LAN1; else if (strncmp((char*)event->data.scalar.value, "USER_LAN2", 9) == 0) *(Ros_UserLan_Port_Setting*)activeItem->valueToSet = CFG_ROS_USER_LAN2; else { - //Note: ideally, we'd disable user lan monitoring here. However, we can't - //guarantee the 'userlan_monitor_enabled' setting won't be parsed after - //this one. If it were to be parsed after 'userlan_monitor_port', we'd - //be disabling it here, only to have it re-enabled later. - //Set the config value to the 'disabled' sentinel value and let the - //validation code below handle the fallout. + // Note: ideally, we'd disable user lan monitoring or set user lan debug + // broadcast to all here. However, we can't guarantee that the 'userlan_monitor_enabled' + // or the 'debug_broadcast_enabled' setting won't be parsed after + // this one. If it were to be parsed after the corresponding port setting, we'd + // be setting it here, only to have it re-set later. Set the config value to the + // 'malformed' sentinel value and let the validation code below handle the fallout. Ros_Debug_BroadcastMsg( - "Unrecognised value for '%s': '%s'. Port monitoring will be disabled", - (char*)event->data.scalar.value, - (char*)activeItem->yamlKey); - *(Ros_UserLan_Port_Setting*)activeItem->valueToSet = CFG_ROS_USER_LAN_DISABLED; + "Unrecognised value for '%s': '%s'.", + (char*)activeItem->yamlKey, + (char*)event->data.scalar.value); + *(Ros_UserLan_Port_Setting*)activeItem->valueToSet = CFG_ROS_USER_LAN_MALFORMED; } #else - #error Unsupported platform +#error Unsupported platform #endif //Note: this logs whatever was in the .yaml, NOT the verified/parsed value above Ros_Debug_BroadcastMsg("Config: %s = %s", (char*)activeItem->yamlKey, @@ -551,7 +557,7 @@ void Ros_ConfigFile_ValidateCriticalSettings() motoRosAssert_withMsg(status == OK, SUBCODE_CONFIGURATION_AGENT_ON_NET_CHECK, "Host on NIC check 1"); -#if defined (YRC1000) || defined (YRC1000u) +#if defined (YRC1000) if (!bAgentOnMySubnet) { //check second lan port @@ -646,7 +652,7 @@ void Ros_ConfigFile_ValidateNonCriticalSettings() } } -#if defined (YRC1000) || defined (YRC1000u) +#if defined (YRC1000) //on these controllers we can try the second interface, if we haven't //already determined we should monitor the first if (g_nodeConfigSettings.userlan_monitor_port == CFG_ROS_USER_LAN_AUTO) @@ -682,15 +688,15 @@ void Ros_ConfigFile_ValidateNonCriticalSettings() //In both cases, verify it's an acceptable value. else { -#if defined (YRC1000) || defined (YRC1000u) +#if defined (YRC1000) if (g_nodeConfigSettings.userlan_monitor_port != CFG_ROS_USER_LAN1 && g_nodeConfigSettings.userlan_monitor_port != CFG_ROS_USER_LAN2) -#elif defined (FS100) || defined (DX200) +#elif defined (FS100) || defined (DX200) || defined (YRC1000u) if (g_nodeConfigSettings.userlan_monitor_port != CFG_ROS_USER_LAN1) #endif { - mpSetAlarm(ALARM_CONFIGURATION_FAIL, "Invalid UserLan port in cfg", + mpSetAlarm(ALARM_CONFIGURATION_FAIL, "Bad UserLan monitor port in cfg", SUBCODE_CONFIGURATION_INVALID_USERLAN_MONITOR_PORT); g_nodeConfigSettings.userlan_monitor_enabled = FALSE; Ros_Debug_BroadcastMsg( @@ -699,6 +705,28 @@ void Ros_ConfigFile_ValidateNonCriticalSettings() } } } + + if (g_nodeConfigSettings.debug_broadcast_enabled) + { + Ros_Debug_BroadcastMsg("UserLan debug broadcast enabled, checking port setting..."); + //Check if the port setting is valid only if the debug broadcast is enabled +#if defined (YRC1000) + if (g_nodeConfigSettings.debug_broadcast_port != CFG_ROS_USER_LAN1 && + g_nodeConfigSettings.debug_broadcast_port != CFG_ROS_USER_LAN2 && + g_nodeConfigSettings.debug_broadcast_port != CFG_ROS_USER_LAN_ALL) +#elif defined (FS100) || defined (DX200) || defined (YRC1000u) + if (g_nodeConfigSettings.debug_broadcast_port != CFG_ROS_USER_LAN1) +#endif + { + mpSetAlarm(ALARM_CONFIGURATION_FAIL, "Bad UserLan debug port in cfg", + SUBCODE_CONFIGURATION_INVALID_DEBUG_BROADCAST_PORT); + Ros_Debug_BroadcastMsg( + "debug_broadcast_port value %d is invalid, broadcasting to all enabled ports instead", + g_nodeConfigSettings.debug_broadcast_port); + } + } + + } const char* const Ros_ConfigFile_Rmw_Qos_ProfileSetting_ToString(Ros_QoS_Profile_Setting val) @@ -758,6 +786,8 @@ void Ros_ConfigFile_PrintActiveConfiguration(Ros_Configuration_Settings const* c Ros_Debug_BroadcastMsg("Config: userlan_monitor_enabled = %d", config->userlan_monitor_enabled); Ros_Debug_BroadcastMsg("Config: userlan_monitor_port = %d", config->userlan_monitor_port); Ros_Debug_BroadcastMsg("Config: ignore_missing_calib_data = %d", config->ignore_missing_calib_data); + Ros_Debug_BroadcastMsg("Config: debug_broadcast_enabled = %d", config->debug_broadcast_enabled); + Ros_Debug_BroadcastMsg("Config: debug_broadcast_port = %d", config->debug_broadcast_port); } void Ros_ConfigFile_Parse() @@ -765,8 +795,6 @@ void Ros_ConfigFile_Parse() BOOL bAlarmOnce = TRUE; BOOL bOkToInit = TRUE; - Ros_ConfigFile_SetAllDefaultValues(); - do { #if defined (FS100) || defined (YRC1000) || defined (YRC1000u) @@ -862,6 +890,16 @@ void Ros_ConfigFile_Parse() Ros_ConfigFile_ValidateCriticalSettings(); Ros_ConfigFile_ValidateNonCriticalSettings(); +#if defined(YRC1000) + // If the debug broadcast is enabled and the user chose a specific port, then + // we can no longer broadcast over ALL, we have to change it to only broadcast over one port + if (g_nodeConfigSettings.debug_broadcast_enabled && + (g_nodeConfigSettings.debug_broadcast_port == CFG_ROS_USER_LAN1 || + g_nodeConfigSettings.debug_broadcast_port == CFG_ROS_USER_LAN2)) + { + Ros_Debug_SetFromConfig(); + } +#endif Ros_ConfigFile_PrintActiveConfiguration(&g_nodeConfigSettings); } diff --git a/src/ConfigFile.h b/src/ConfigFile.h index 17702861..fb903356 100644 --- a/src/ConfigFile.h +++ b/src/ConfigFile.h @@ -88,17 +88,25 @@ typedef enum typedef enum { - CFG_ROS_USER_LAN_DISABLED = -2, //sentinel + CFG_ROS_USER_LAN_MALFORMED = -3, //sentinel + CFG_ROS_USER_LAN_ALL = -2, //sentinel CFG_ROS_USER_LAN_AUTO = -1, //sentinel CFG_ROS_USER_LAN1 = ROS_USER_LAN1, CFG_ROS_USER_LAN2 = ROS_USER_LAN2, } Ros_UserLan_Port_Setting; -#define DEFAULT_ULAN_MON_ENABLED TRUE -#define DEFAULT_ULAN_MON_LINK CFG_ROS_USER_LAN_AUTO +#define DEFAULT_ULAN_MON_ENABLED TRUE +#define DEFAULT_ULAN_MON_LINK CFG_ROS_USER_LAN_AUTO -#define DEFAULT_IGNORE_MISSING_CALIB FALSE +#define DEFAULT_IGNORE_MISSING_CALIB FALSE +#define DEFAULT_ULAN_DEBUG_BROADCAST_ENABLED TRUE + +#if defined (YRC1000) +#define DEFAULT_ULAN_DEBUG_BROADCAST_PORT CFG_ROS_USER_LAN_ALL +#else +#define DEFAULT_ULAN_DEBUG_BROADCAST_PORT CFG_ROS_USER_LAN1 +#endif typedef struct { //TODO(gavanderhoorn): add support for unsigned types @@ -141,10 +149,15 @@ typedef struct Ros_UserLan_Port_Setting userlan_monitor_port; BOOL ignore_missing_calib_data; + + BOOL debug_broadcast_enabled; + Ros_UserLan_Port_Setting debug_broadcast_port; } Ros_Configuration_Settings; extern Ros_Configuration_Settings g_nodeConfigSettings; +extern void Ros_ConfigFile_SetAllDefaultValues(); + extern void Ros_ConfigFile_Parse(); extern rmw_qos_profile_t const* const Ros_ConfigFile_To_Rmw_Qos_Profile(Ros_QoS_Profile_Setting val); diff --git a/src/ControllerStatusIO.c b/src/ControllerStatusIO.c index dda96847..2cf420b1 100644 --- a/src/ControllerStatusIO.c +++ b/src/ControllerStatusIO.c @@ -315,6 +315,11 @@ BOOL Ros_Controller_IsAlarm() || (g_Ros_Controller.ioStatus[IO_ROBOTSTATUS_ALARM_USER]!=0) ); } +BOOL Ros_Controller_IsMajorAlarm() +{ + return ((g_Ros_Controller.ioStatus[IO_ROBOTSTATUS_ALARM_MAJOR] != 0)); +} + BOOL Ros_Controller_IsError() { return ((g_Ros_Controller.ioStatus[IO_ROBOTSTATUS_ERROR]!=0)); @@ -378,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 @@ -418,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; @@ -436,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; @@ -462,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 0b95a5a2..22865a2d 100644 --- a/src/ControllerStatusIO.h +++ b/src/ControllerStatusIO.h @@ -111,6 +111,7 @@ extern void Ros_Controller_StatusInit(); extern BOOL Ros_Controller_StatusRead(USHORT ioStatus[IO_ROBOTSTATUS_MAX]); extern BOOL Ros_Controller_IoStatusUpdate(); extern BOOL Ros_Controller_IsAlarm(); +extern BOOL Ros_Controller_IsMajorAlarm(); extern BOOL Ros_Controller_IsError(); extern BOOL Ros_Controller_IsPlay(); extern BOOL Ros_Controller_IsTeach(); @@ -128,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/Debug.c b/src/Debug.c index f720eb32..d89a52dc 100644 --- a/src/Debug.c +++ b/src/Debug.c @@ -14,9 +14,18 @@ #define FORMATTED_TIME_SIZE 1024 -int ros_DebugSocket = -1; -struct sockaddr_in ros_debug_destAddr1; +typedef struct +{ + UINT8 enabledPortCount; + int debugSocket[MAX_NETWORK_PORTS]; + struct sockaddr_in destAddr[MAX_NETWORK_PORTS]; +} user_lan_info_t; + +user_lan_info_t ros_debugPorts = {0}; +// This sets the host to broadcast the message on all ports. +// This behavior will only be overwritten if the user specifies otherwise +// in the config file. void Ros_Debug_Init() { ULONG ip_be; @@ -24,15 +33,76 @@ void Ros_Debug_Init() ULONG gateway_be; int broadcastVal = 1; UCHAR mac[6]; + STATUS status; + int count = 0; + int* socket = ros_debugPorts.debugSocket; + struct sockaddr_in* sin = ros_debugPorts.destAddr; + + bzero(&ros_debugPorts, sizeof(ros_debugPorts)); + + for (int i = 1; i <= MAX_NETWORK_PORTS; i++) + { + + status = Ros_mpNICData(i, &ip_be, &subnetmask_be, mac, &gateway_be); + + if (status == OK) + { + socket[count] = mpSocket(AF_INET, SOCK_DGRAM, 0); + Ros_setsockopt(socket[count], SOL_SOCKET, SO_BROADCAST, (char*)&broadcastVal, sizeof(broadcastVal)); + sin[count].sin_addr.s_addr = ip_be | (~subnetmask_be); + sin[count].sin_family = AF_INET; + sin[count].sin_port = mpHtons(DEBUG_UDP_PORT_NUMBER); + count++; + } + } + ros_debugPorts.enabledPortCount = count; +} + +// This sets the host to broadcast the message only to the one port specified in the cfg file. +// This overwrites the behavior from Ros_Debug_Init() so it will no longer broadcast +// to all ports. +void Ros_Debug_SetFromConfig() +{ + ULONG ip_be; + ULONG subnetmask_be; + ULONG gateway_be; + int broadcastVal = 1; + UCHAR mac[6]; + STATUS status; + char message[ERROR_MSG_MAX_SIZE]; + int* socket = ros_debugPorts.debugSocket; + struct sockaddr_in* sin = ros_debugPorts.destAddr; - ros_DebugSocket = mpSocket(AF_INET, SOCK_DGRAM, 0); - Ros_setsockopt(ros_DebugSocket, SOL_SOCKET, SO_BROADCAST, (char*)&broadcastVal, sizeof(broadcastVal)); + bzero(&ros_debugPorts, sizeof(ros_debugPorts)); - Ros_mpNICData(ROS_USER_LAN1, &ip_be, &subnetmask_be, mac, &gateway_be); + status = Ros_mpNICData(g_nodeConfigSettings.debug_broadcast_port, &ip_be, &subnetmask_be, mac, &gateway_be); - ros_debug_destAddr1.sin_addr.s_addr = ip_be | (~subnetmask_be); - ros_debug_destAddr1.sin_family = AF_INET; - ros_debug_destAddr1.sin_port = mpHtons(DEBUG_UDP_PORT_NUMBER); + // This function is only called if the user specifies which port the host should broadcast to. So + // only one socket needs to have its information populated in this case. + if (status == OK) + { + socket[0] = mpSocket(AF_INET, SOCK_DGRAM, 0); + Ros_setsockopt(socket[0], SOL_SOCKET, SO_BROADCAST, (char*)&broadcastVal, sizeof(broadcastVal)); + sin[0].sin_addr.s_addr = ip_be | (~subnetmask_be); + sin[0].sin_family = AF_INET; + sin[0].sin_port = mpHtons(DEBUG_UDP_PORT_NUMBER); + ros_debugPorts.enabledPortCount = 1; + } + else + { + //If it fails, then disable debug broadcast and notify the user via an alarm. + int ret = snprintf(message, ERROR_MSG_MAX_SIZE, "Enable LAN port %d for debug", g_nodeConfigSettings.debug_broadcast_port); + if (0 < ret && ret <= ERROR_MSG_MAX_SIZE) + { + mpSetAlarm(ALARM_ASSERTION_FAIL, message, SUBCODE_DEBUG_INIT_FAIL_MP_NICDATA); + } + else + { + mpSetAlarm(ALARM_ASSERTION_FAIL, "Enable debug LAN port from cfg", SUBCODE_DEBUG_INIT_FAIL_MP_NICDATA); + } + g_nodeConfigSettings.debug_broadcast_enabled = FALSE; + ros_debugPorts.enabledPortCount = 0; + } } void Ros_Debug_BroadcastMsg(char* fmt, ...) @@ -40,15 +110,25 @@ void Ros_Debug_BroadcastMsg(char* fmt, ...) char str[MAX_DEBUG_MESSAGE_SIZE]; va_list va; + if (g_nodeConfigSettings.debug_broadcast_enabled && ros_debugPorts.enabledPortCount == 0) + { + Ros_Debug_Init(); + } + + //Only exit if the broadcast is not enabled and log_to_stdout is false. The message that is built + //for each of those is the same, so as long as it will either be broadcast or logged to stdout, + //the message should be built + if (!g_nodeConfigSettings.debug_broadcast_enabled && !g_nodeConfigSettings.log_to_stdout) + { + return; + } + bzero(str, MAX_DEBUG_MESSAGE_SIZE); va_start(va, fmt); vsnprintf(str, MAX_DEBUG_MESSAGE_SIZE, fmt, va); va_end(va); - if (ros_DebugSocket == -1) - Ros_Debug_Init(); - // Timestamp //The timestamp for the message "Found Micro-Ros PC Agent" will be the epoch time (THU 1970-01-01 00:00:00.000) as the global flags //are set to indicate that the Micro-Ros PC Agent is connected but the first sync of the host time using the micro-ROS agent is yet to occur @@ -82,7 +162,15 @@ void Ros_Debug_BroadcastMsg(char* fmt, ...) memcpy(str, timestamp, timestamp_length); } - mpSendTo(ros_DebugSocket, str, strlen(str), 0, (struct sockaddr*) &ros_debug_destAddr1, sizeof(struct sockaddr_in)); + + if (g_nodeConfigSettings.debug_broadcast_enabled) + { + for (int i = 0; i < ros_debugPorts.enabledPortCount; i++) + { + mpSendTo(ros_debugPorts.debugSocket[i], str, strlen(str), 0, (struct sockaddr*)&(ros_debugPorts.destAddr[i]), sizeof(struct sockaddr_in)); + } + } + if (g_nodeConfigSettings.log_to_stdout) puts(str); diff --git a/src/Debug.h b/src/Debug.h index a37e02e0..dc8ddfbc 100644 --- a/src/Debug.h +++ b/src/Debug.h @@ -7,7 +7,13 @@ #ifndef MOTOROS2_DEBUG_H #define MOTOROS2_DEBUG_H +#if defined (YRC1000) +#define MAX_NETWORK_PORTS 2 +#else +#define MAX_NETWORK_PORTS 1 +#endif +extern void Ros_Debug_SetFromConfig(); extern void Ros_Debug_BroadcastMsg(char* fmt, ...); extern void Ros_Debug_LogToConsole(char* fmt, ...); diff --git a/src/ErrorHandling.c b/src/ErrorHandling.c index 195f0738..8f0b9daf 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"; @@ -71,11 +72,61 @@ const char* const Ros_ErrorHandling_MotionNotReadyCode_ToString(MotionNotReadyCo return motoros2_interfaces__msg__MotionReadyEnum__NOT_READY_OTHER_TRAJ_MODE_ACTIVE_STR; case MOTION_NOT_READY_NOT_CONT_CYCLE_MODE: 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; } } +const char* const Ros_ErrorHandling_Init_Trajectory_Status_ToString(Init_Trajectory_Status code) +{ + //messages defined in motoros2_interfaces/msg/InitTrajEnum.msg + switch (code) + { + case INIT_TRAJ_OK: + return motoros2_interfaces__msg__InitTrajEnum__INIT_TRAJ_OK_STR; + case INIT_TRAJ_UNSPECIFIED: + return motoros2_interfaces__msg__InitTrajEnum__INIT_TRAJ_UNSPECIFIED_STR; + case INIT_TRAJ_TOO_SMALL: + return motoros2_interfaces__msg__InitTrajEnum__INIT_TRAJ_TOO_SMALL_STR; + case INIT_TRAJ_TOO_BIG: + return motoros2_interfaces__msg__InitTrajEnum__INIT_TRAJ_TOO_BIG_STR; + case INIT_TRAJ_ALREADY_IN_MOTION: + return motoros2_interfaces__msg__InitTrajEnum__INIT_TRAJ_ALREADY_IN_MOTION_STR; + case INIT_TRAJ_INVALID_STARTING_POS: + return motoros2_interfaces__msg__InitTrajEnum__INIT_TRAJ_INVALID_STARTING_POS_STR; + case INIT_TRAJ_INVALID_VELOCITY: + return motoros2_interfaces__msg__InitTrajEnum__INIT_TRAJ_INVALID_VELOCITY_STR; + case INIT_TRAJ_INVALID_JOINTNAME: + return motoros2_interfaces__msg__InitTrajEnum__INIT_TRAJ_INVALID_JOINTNAME_STR; + case INIT_TRAJ_INCOMPLETE_JOINTLIST: + return motoros2_interfaces__msg__InitTrajEnum__INIT_TRAJ_INCOMPLETE_JOINTLIST_STR; + case INIT_TRAJ_INVALID_TIME: + return motoros2_interfaces__msg__InitTrajEnum__INIT_TRAJ_INVALID_TIME_STR; + case INIT_TRAJ_WRONG_MODE: + return motoros2_interfaces__msg__InitTrajEnum__INIT_TRAJ_WRONG_MODE_STR; + case INIT_TRAJ_BACKWARD_TIME: + return motoros2_interfaces__msg__InitTrajEnum__INIT_TRAJ_BACKWARD_TIME_STR; + case INIT_TRAJ_WRONG_NUMBER_OF_POSITIONS: + return motoros2_interfaces__msg__InitTrajEnum__INIT_TRAJ_WRONG_NUMBER_OF_POSITIONS_STR; + case INIT_TRAJ_WRONG_NUMBER_OF_VELOCITIES: + return motoros2_interfaces__msg__InitTrajEnum__INIT_TRAJ_WRONG_NUMBER_OF_VELOCITIES_STR; + case INIT_TRAJ_INVALID_ENDING_VELOCITY: + return motoros2_interfaces__msg__InitTrajEnum__INIT_TRAJ_INVALID_ENDING_VELOCITY_STR; + case INIT_TRAJ_INVALID_ENDING_ACCELERATION: + return motoros2_interfaces__msg__InitTrajEnum__INIT_TRAJ_INVALID_ENDING_ACCELERATION_STR; + case INIT_TRAJ_DUPLICATE_JOINT_NAME: + return motoros2_interfaces__msg__InitTrajEnum__INIT_TRAJ_DUPLICATE_JOINT_NAME_STR; + default: + return motoros2_interfaces__msg__InitTrajEnum__INIT_TRAJ_UNSPECIFIED_STR; + } +} + void motoRosAssert(BOOL mustBeTrue, ALARM_ASSERTION_FAIL_SUBCODE subCodeIfFalse) { motoRosAssert_withMsg(mustBeTrue, subCodeIfFalse, APPLICATION_NAME ": Fatal Error"); diff --git a/src/ErrorHandling.h b/src/ErrorHandling.h index 98362d26..81431c83 100644 --- a/src/ErrorHandling.h +++ b/src/ErrorHandling.h @@ -32,26 +32,30 @@ typedef enum MOTION_NOT_READY_OTHER_PROGRAM_RUNNING = motoros2_interfaces__msg__MotionReadyEnum__NOT_READY_OTHER_PROGRAM_RUNNING, 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 { - INIT_TRAJ_OK = 0, - INIT_TRAJ_TOO_SMALL = 200, - INIT_TRAJ_TOO_BIG, - INIT_TRAJ_ALREADY_IN_MOTION, - INIT_TRAJ_INVALID_STARTING_POS, - INIT_TRAJ_INVALID_VELOCITY, - INIT_TRAJ_INVALID_JOINTNAME, - INIT_TRAJ_INCOMPLETE_JOINTLIST, - INIT_TRAJ_INVALID_TIME, - INIT_TRAJ_WRONG_MODE, - INIT_TRAJ_BACKWARD_TIME, - INIT_TRAJ_WRONG_NUMBER_OF_POSITIONS, - INIT_TRAJ_WRONG_NUMBER_OF_VELOCITIES, - INIT_TRAJ_INVALID_ENDING_VELOCITY, - INIT_TRAJ_INVALID_ENDING_ACCELERATION, - INIT_TRAJ_DUPLICATE_JOINT_NAME, + INIT_TRAJ_OK = motoros2_interfaces__msg__InitTrajEnum__INIT_TRAJ_OK, + INIT_TRAJ_UNSPECIFIED = motoros2_interfaces__msg__InitTrajEnum__INIT_TRAJ_UNSPECIFIED, + INIT_TRAJ_TOO_SMALL = motoros2_interfaces__msg__InitTrajEnum__INIT_TRAJ_TOO_SMALL, + INIT_TRAJ_TOO_BIG = motoros2_interfaces__msg__InitTrajEnum__INIT_TRAJ_TOO_BIG, + INIT_TRAJ_ALREADY_IN_MOTION = motoros2_interfaces__msg__InitTrajEnum__INIT_TRAJ_ALREADY_IN_MOTION, + INIT_TRAJ_INVALID_STARTING_POS = motoros2_interfaces__msg__InitTrajEnum__INIT_TRAJ_INVALID_STARTING_POS, + INIT_TRAJ_INVALID_VELOCITY = motoros2_interfaces__msg__InitTrajEnum__INIT_TRAJ_INVALID_VELOCITY, + INIT_TRAJ_INVALID_JOINTNAME = motoros2_interfaces__msg__InitTrajEnum__INIT_TRAJ_INVALID_JOINTNAME, + INIT_TRAJ_INCOMPLETE_JOINTLIST = motoros2_interfaces__msg__InitTrajEnum__INIT_TRAJ_INCOMPLETE_JOINTLIST, + INIT_TRAJ_INVALID_TIME = motoros2_interfaces__msg__InitTrajEnum__INIT_TRAJ_INVALID_TIME, + INIT_TRAJ_WRONG_MODE = motoros2_interfaces__msg__InitTrajEnum__INIT_TRAJ_WRONG_MODE, + INIT_TRAJ_BACKWARD_TIME = motoros2_interfaces__msg__InitTrajEnum__INIT_TRAJ_BACKWARD_TIME, + INIT_TRAJ_WRONG_NUMBER_OF_POSITIONS = motoros2_interfaces__msg__InitTrajEnum__INIT_TRAJ_WRONG_NUMBER_OF_POSITIONS, + INIT_TRAJ_WRONG_NUMBER_OF_VELOCITIES = motoros2_interfaces__msg__InitTrajEnum__INIT_TRAJ_WRONG_NUMBER_OF_VELOCITIES, + INIT_TRAJ_INVALID_ENDING_VELOCITY = motoros2_interfaces__msg__InitTrajEnum__INIT_TRAJ_INVALID_ENDING_VELOCITY, + INIT_TRAJ_INVALID_ENDING_ACCELERATION = motoros2_interfaces__msg__InitTrajEnum__INIT_TRAJ_INVALID_ENDING_ACCELERATION, + INIT_TRAJ_DUPLICATE_JOINT_NAME = motoros2_interfaces__msg__InitTrajEnum__INIT_TRAJ_DUPLICATE_JOINT_NAME, } Init_Trajectory_Status; typedef enum @@ -170,6 +174,8 @@ typedef enum SUBCODE_FAIL_MP_NICDATA_INIT1, SUBCODE_FAIL_INVALID_BASE_TRACK_MOTION_TYPE, SUBCODE_FAIL_NODE_TYPE_CACHE_INIT, + SUBCODE_DEBUG_INIT_FAIL_MP_NICDATA, + } ALARM_ASSERTION_FAIL_SUBCODE; //8011 typedef enum @@ -199,6 +205,7 @@ typedef enum SUBCODE_CONFIGURATION_USERLAN_MONITOR_AUTO_DETECT_FAILED, SUBCODE_CONFIGURATION_RUNTIME_USERLAN_LINKUP_ERR, SUBCODE_CONFIGURATION_NO_CALIB_FILES_LOADED, + SUBCODE_CONFIGURATION_INVALID_DEBUG_BROADCAST_PORT, } ALARM_CONFIGURATION_FAIL_SUBCODE; //8013 typedef enum @@ -228,5 +235,6 @@ extern void motoRosAssert_withMsg(BOOL mustBeTrue, ALARM_ASSERTION_FAIL_SUBCODE extern const char* const Ros_ErrorHandling_ErrNo_ToString(int errNo); extern const char* const Ros_ErrorHandling_MotionNotReadyCode_ToString(MotionNotReadyCode code); +extern const char* const Ros_ErrorHandling_Init_Trajectory_Status_ToString(Init_Trajectory_Status code); #endif // MOTOROS2_ERROR_HANDLING_H 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; checkCount #include #include +#include #include #include #include diff --git a/src/ServiceResetError.c b/src/ServiceResetError.c index b78588e8..3b1ed316 100644 --- a/src/ServiceResetError.c +++ b/src/ServiceResetError.c @@ -83,7 +83,15 @@ void Ros_ServiceResetError_Trigger(const void* request_msg, void* response_msg) } } - // Check for condition that can be fixed remotely + // Major alarms cannot be reset remotely + if (Ros_Controller_IsMajorAlarm()) + { + rosidl_runtime_c__String__assign(&response->message, "Major alarm active. Cannot be reset. Check teach pendant"); + response->result_code.value = MOTION_NOT_READY_MAJOR_ALARM; + goto DONE; + } + + // Other alarm types can be reset remotely if (Ros_Controller_IsAlarm()) { // Reset alarm diff --git a/src/ServiceSelectMotionTool.c b/src/ServiceSelectMotionTool.c index b7fdf107..fd43d97d 100644 --- a/src/ServiceSelectMotionTool.c +++ b/src/ServiceSelectMotionTool.c @@ -83,7 +83,7 @@ void Ros_ServiceSelectMotionTool_Trigger(const void* request_msg, void* response } //make sure a valid tool is specified (tool_number is unsigned, so cannot be < 0) - if (request->tool_number >= MAX_VALID_TOOL_INDEX) + if (request->tool_number > MAX_VALID_TOOL_INDEX) { rosidl_runtime_c__String__assign(&response->message, motoros2_interfaces__msg__SelectionResultCodes__INVALID_SELECTION_INDEX_STR); response->result_code.value = motoros2_interfaces__msg__SelectionResultCodes__INVALID_SELECTION_INDEX; diff --git a/src/ServiceStartPointQueueMode.c b/src/ServiceStartPointQueueMode.c index f7f1d61e..3adf8dc4 100644 --- a/src/ServiceStartPointQueueMode.c +++ b/src/ServiceStartPointQueueMode.c @@ -53,21 +53,14 @@ void Ros_ServiceStartPointQueueMode_Trigger(const void* request_msg, void* respo response->result_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)); diff --git a/src/main.c b/src/main.c index cef21928..0b73b202 100644 --- a/src/main.c +++ b/src/main.c @@ -53,6 +53,8 @@ void RosInitTask() { g_Ros_Controller.tidIncMoveThread = INVALID_TASK; + Ros_ConfigFile_SetAllDefaultValues(); + //Check to see if another version of MotoROS2.out is running on this controller. motoRosAssert_withMsg(!Ros_IsOtherInstanceRunning(), SUBCODE_MULTIPLE_INSTANCES_DETECTED, "MotoROS2 - Multiple Instances");