Add debug broadcast enable/disable option and debug broadcast port op…

…tion to config file

config/motoros2_config.yaml

@@ -284,3 +284,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 provided in the distribution of MotoROS2 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
+#userlan_debug_broadcast_enabled: true
+
+#-----------------------------------------------------------------------------
+# Which network port should MotoROS2 broadcast the debug messages to, if 
+# 'userlan_debug_broadcast_enabled' is 'true'?
+#
+# If not specified and 'userlan_debug_broadcast_enabled' is true, MotoROS2 will 
+# send the broadcast to all network ports which are enabled on the controller. 
+#
+# To choose a specific port to broadcast debug messages, uncomment 
+# 'userlan_debug_broadcast_port' below and set it to the desired port.
+#
+# NOTE 1:   this setting only applies to YRC1000 and YRC1000u controllers.
+#         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)
+#userlan_debug_broadcast_port: USER_LAN1

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 },
+    { "userlan_debug_broadcast_enabled", &g_nodeConfigSettings.userlan_debug_broadcast_enabled, Value_Bool },
+    { "userlan_debug_broadcast_port", &g_nodeConfigSettings.userlan_debug_broadcast_port, Value_UserLanPort },
 };
 
 void Ros_ConfigFile_SetAllDefaultValues()
@@ -241,6 +243,10 @@ void Ros_ConfigFile_SetAllDefaultValues()
 
     //ignore_missing_calib_data
     g_nodeConfigSettings.ignore_missing_calib_data = DEFAULT_IGNORE_MISSING_CALIB;
+
+    //userlan debug broadcast
+    g_nodeConfigSettings.userlan_debug_broadcast_enabled = DEFAULT_ULAN_DEBUG_BROADCAST_ENABLED;
+    g_nodeConfigSettings.userlan_debug_broadcast_port = DEFAULT_ULAN_DEBUG_BROADCAST_PORT;
 }
 
 void Ros_ConfigFile_CheckYamlEvent(yaml_event_t* event)
@@ -327,7 +333,7 @@ void Ros_ConfigFile_CheckYamlEvent(yaml_event_t* event)
                 case Value_UserLanPort:
 #if defined (FS100) || defined (DX200)
                     // single port, override whatever was configured
-                    *(Ros_UserLan_Port_Setting*)activeItem->valueToSet = CFG_ROS_USER_LAN1;
+                    * (Ros_UserLan_Port_Setting*)activeItem->valueToSet = CFG_ROS_USER_LAN1;
                     Ros_Debug_BroadcastMsg("DX200 or FS100: override to 'USER_LAN1'");
 
 #elif defined (YRC1000) || defined (YRC1000u)
@@ -337,20 +343,22 @@ void Ros_ConfigFile_CheckYamlEvent(yaml_event_t* event)
                         *(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
+                        //Note: ideally, we'd disable user lan monitoring or user lan debug
+                        //broadcast here. However, we can't guarantee the 'userlan_monitor_enabled' 
+                        // or the 'userlan_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 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.
                         Ros_Debug_BroadcastMsg(
-                            "Unrecognised value for '%s': '%s'. Port monitoring will be disabled",
+                            "Unrecognised value for '%s': '%s'. '%s' will be disabled",
+                            (char*)activeItem->yamlKey,
                             (char*)event->data.scalar.value,
                             (char*)activeItem->yamlKey);
                         *(Ros_UserLan_Port_Setting*)activeItem->valueToSet = CFG_ROS_USER_LAN_DISABLED;
                     }
 #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,
@@ -690,7 +698,7 @@ void Ros_ConfigFile_ValidateNonCriticalSettings()
             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 +707,29 @@ void Ros_ConfigFile_ValidateNonCriticalSettings()
             }
         }
     }
+
+    if (g_nodeConfigSettings.userlan_debug_broadcast_enabled)
+    {
+        Ros_Debug_BroadcastMsg("UserLan debug broadcast enabled, checking port setting...");
+
+#if defined (YRC1000) || defined (YRC1000u)
+        if (g_nodeConfigSettings.userlan_debug_broadcast_port != CFG_ROS_USER_LAN1 &&
+            g_nodeConfigSettings.userlan_debug_broadcast_port != CFG_ROS_USER_LAN2 && 
+            g_nodeConfigSettings.userlan_debug_broadcast_port != CFG_ROS_USER_LAN_ALL)
+#elif defined (FS100) || defined (DX200)
+        if (g_nodeConfigSettings.userlan_debug_broadcast_port != CFG_ROS_USER_LAN1)
+#endif
+        {
+            mpSetAlarm(ALARM_CONFIGURATION_FAIL, "Bad UserLan debug port in cfg",
+                SUBCODE_CONFIGURATION_INVALID_USERLAN_DEBUG_BROADCAST_PORT);
+            Ros_Debug_BroadcastMsg(
+                "userlan_debug_broadcast_port value %d is invalid, disabling debug broadcast",
+                g_nodeConfigSettings.userlan_debug_broadcast_port);
+            g_nodeConfigSettings.userlan_debug_broadcast_enabled = FALSE;
+        }
+    }
+
+
 }
 
 const char* const Ros_ConfigFile_Rmw_Qos_ProfileSetting_ToString(Ros_QoS_Profile_Setting val)
@@ -862,6 +893,12 @@ void Ros_ConfigFile_Parse()
 
     Ros_ConfigFile_ValidateCriticalSettings();
     Ros_ConfigFile_ValidateNonCriticalSettings();
+#if defined(YRC1000) || defined(YRC1000u)
+    if(g_nodeConfigSettings.userlan_debug_broadcast_enabled &&
+        (g_nodeConfigSettings.userlan_debug_broadcast_port == CFG_ROS_USER_LAN1 ||
+        g_nodeConfigSettings.userlan_debug_broadcast_port == CFG_ROS_USER_LAN2))
+        Ros_Debug_SetFromConfig();
+#endif
     Ros_ConfigFile_PrintActiveConfiguration(&g_nodeConfigSettings);
 }
 

src/ConfigFile.h

@@ -88,17 +88,20 @@ typedef enum
 
 typedef enum
 {
-    CFG_ROS_USER_LAN_DISABLED = -2,  //sentinel
+    CFG_ROS_USER_LAN_DISABLED = -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
+#define DEFAULT_ULAN_DEBUG_BROADCAST_PORT        CFG_ROS_USER_LAN_ALL
 typedef struct
 {
     //TODO(gavanderhoorn): add support for unsigned types
@@ -141,6 +144,9 @@ typedef struct
     Ros_UserLan_Port_Setting userlan_monitor_port;
 
     BOOL ignore_missing_calib_data;
+
+    BOOL userlan_debug_broadcast_enabled;
+    Ros_UserLan_Port_Setting userlan_debug_broadcast_port;
 } Ros_Configuration_Settings;
 
 extern Ros_Configuration_Settings g_nodeConfigSettings;

src/Debug.c

@@ -14,8 +14,14 @@
 
 #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];
+} userLanInfo;
+
+userLanInfo debugPorts = {0};
 
 void Ros_Debug_Init()
 {
@@ -24,31 +30,91 @@ void Ros_Debug_Init()
     ULONG gateway_be;
     int broadcastVal = 1;
     UCHAR mac[6];
+    STATUS status;
+    int count = 0;
+    int* socket = debugPorts.debugSocket;
+    struct sockaddr_in* sin = debugPorts.destAddr;
+
+    bzero(&debugPorts, sizeof(debugPorts));
+
+    for (int i = 1; i <= MAX_NETWORK_PORTS; i++)
+    {
+        socket[count] = mpSocket(AF_INET, SOCK_DGRAM, 0);
+        Ros_setsockopt(socket[count], SOL_SOCKET, SO_BROADCAST, (char*)&broadcastVal, sizeof(broadcastVal));
+
+        status = Ros_mpNICData(i, &ip_be, &subnetmask_be, mac, &gateway_be);
+
+        if (status == OK)
+        {
+            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++;
+        }
+    }
+    debugPorts.enabledPortCount = count;
+    if (count < 1)
+    {
+        mpSetAlarm(ALARM_CONFIGURATION_FAIL, "Must enable ETHERNET function", SUBCODE_DEBUG_INIT_FAIL_MP_NICDATA_ALL);
+        g_nodeConfigSettings.userlan_debug_broadcast_enabled = FALSE;
+    }
+}
+
+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 = debugPorts.debugSocket;
+    struct sockaddr_in* sin = debugPorts.destAddr;
 
-    ros_DebugSocket = mpSocket(AF_INET, SOCK_DGRAM, 0);
-    Ros_setsockopt(ros_DebugSocket, SOL_SOCKET, SO_BROADCAST, (char*)&broadcastVal, sizeof(broadcastVal));
+    bzero(&debugPorts, sizeof(debugPorts));
 
-    Ros_mpNICData(ROS_USER_LAN1, &ip_be, &subnetmask_be, mac, &gateway_be);
+    socket[0] = mpSocket(AF_INET, SOCK_DGRAM, 0);
+    Ros_setsockopt(socket[0], SOL_SOCKET, SO_BROADCAST, (char*)&broadcastVal, sizeof(broadcastVal));
 
-    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);
+    status = Ros_mpNICData(g_nodeConfigSettings.userlan_debug_broadcast_port, &ip_be, &subnetmask_be, mac, &gateway_be);
+
+    if (status == OK)
+    {
+        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);
+        debugPorts.enabledPortCount = 1;
+    }
+
+    if (debugPorts.enabledPortCount < 1)
+    {
+        int ret = snprintf(message, ERROR_MSG_MAX_SIZE, "Enable LAN port %d for debug", g_nodeConfigSettings.userlan_debug_broadcast_port);
+        if (0 < ret && ret <= 32)
+            mpSetAlarm(ALARM_CONFIGURATION_FAIL, message, SUBCODE_DEBUG_INIT_FAIL_MP_NICDATA_SPECIFIC);
+        else
+            mpSetAlarm(ALARM_CONFIGURATION_FAIL, "Enable debug LAN port from cfg", SUBCODE_DEBUG_INIT_FAIL_MP_NICDATA_SPECIFIC);
+        g_nodeConfigSettings.userlan_debug_broadcast_enabled = FALSE;
+    }
 }
 
 void Ros_Debug_BroadcastMsg(char* fmt, ...)
 {
     char str[MAX_DEBUG_MESSAGE_SIZE];
     va_list va;
 
+    if (g_nodeConfigSettings.userlan_debug_broadcast_enabled && debugPorts.enabledPortCount == 0)
+        Ros_Debug_Init();
+
+    if (!g_nodeConfigSettings.userlan_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 +148,9 @@ 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));
+    for (int i = 0; i < debugPorts.enabledPortCount; i++)
+        mpSendTo(debugPorts.debugSocket[i], str, strlen(str), 0, (struct sockaddr*)&(debugPorts.destAddr[i]), sizeof(struct sockaddr_in));
+
 
     if (g_nodeConfigSettings.log_to_stdout)
         puts(str);

src/Debug.h

@@ -7,7 +7,13 @@
 
 #ifndef MOTOROS2_DEBUG_H
 #define MOTOROS2_DEBUG_H
+#if defined (YRC1000) || defined (YRC1000u)
+#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, ...);
 

src/ErrorHandling.h

@@ -170,6 +170,8 @@ typedef enum
     SUBCODE_CONFIGURATION_FAIL_MP_NICDATA1,
     SUBCODE_FAIL_MP_NICDATA_INIT1,
     SUBCODE_FAIL_INVALID_BASE_TRACK_MOTION_TYPE,
+    SUBCODE_DEBUG_INIT_FAIL_MP_NICDATA_ALL,
+    SUBCODE_DEBUG_INIT_FAIL_MP_NICDATA_SPECIFIC,
 
 } ALARM_ASSERTION_FAIL_SUBCODE; //8011
 
@@ -200,6 +202,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_USERLAN_DEBUG_BROADCAST_PORT,
 } ALARM_CONFIGURATION_FAIL_SUBCODE; //8013
 
 typedef enum