Raise alarm if calib data can't be loaded on multi-group systems with TF enabled

config/motoros2_config.yaml

@@ -268,3 +268,19 @@ publisher_qos:
 #
 # DEFAULT: false
 #allow_custom_inform: false
+
+#-----------------------------------------------------------------------------
+# Ignore missing calibration data on multi-group systems with TF broadcast
+# enabled.
+#
+# MotoROS2 will raise an alarm (8013[16]) on a multi-group controller if it
+# is unable to load any calibration data and broadcasting of TF frames is
+# enabled.
+# This calibration data is needed to correctly disambiguate multiple distinct
+# TF trees which would otherwise potentially (partially) overlap each other.
+#
+# Set this flag to 'true' to prevent the alarm from being raised, for instance
+# in cases where uncalibrated multi-group systems should still broadcast TF.
+#
+# DEFAULT: false
+#ignore_missing_calib_data: false

doc/troubleshooting.md

@@ -989,6 +989,39 @@ If the behavior persists, save a copy of the output of the [debug-listener scrip
 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: 8013[16]
+
+*Example:*
+
+```text
+ALARM 8013
+ No calibration: invalid TF
+[16]
+```
+
+*Solution:*
+MotoROS2 was unable to load any kinematic calibration data during initialisation.
+This calibration data is used to update the origins of TF frames broadcast by MotoROS2 if TF broadcasts are enabled.
+
+Without (valid) calibration data the origins of distinct TF trees might overlap (see [Incorrect transform tree origin with multi-robot setups](../README.md#incorrect-transform-tree-origin-with-multi-robot-setups)), creating potentially dangerous situations when that TF data is consumed by applications which for example use it for collision avoidance motion planning.
+
+This alarm is only raised if all of the following conditions are true:
+
+1. MotoROS2 is configured to broadcast TF (`publish_tf` is `true`)
+1. the controller is configured with multiple motion groups (ie: multiple robots)
+1. none of the motion groups have been calibrated against each other
+
+The alarm can be prevented by performing (robot) group calibration or by disabling TF broadcasts (set `publish_tf` to `false`).
+
+In case TF broadcasting for uncalibrated multi-group systems is still desired, the alarm can be disabled by setting the `ignore_missing_calib_data` item in the MotoROS2 configuration to `true` (default is: `false`).
+When disabling the alarm, make absolutely sure consuming applications are capable of disambiguating potentially overlapping TF trees.
+
+In case of any updates to the configuration file, [changes will need to be propagated to the Yaskawa controller](../README.md#updating-the-configuration).
+
+In case the alarm is still raised after calibration was performed, TF broadcasting was disabled and/or the alarm was disabled, save a copy of the output of the [debug-listener script](#debug-log-client) and the `PANELBOX.LOG` and `RBCALIB.DAT` files from the robot's teach pendant.
+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: 8014[0]
 
 *Example:*

src/ConfigFile.c

@@ -120,6 +120,7 @@ Configuration_Item Ros_ConfigFile_Items[] =
     { "allow_custom_inform_job", &g_nodeConfigSettings.allow_custom_inform_job, Value_Bool },
     { "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 },
 };
 
 void Ros_ConfigFile_SetAllDefaultValues()
@@ -237,6 +238,9 @@ void Ros_ConfigFile_SetAllDefaultValues()
     //userlan monitoring
     g_nodeConfigSettings.userlan_monitor_enabled = DEFAULT_ULAN_MON_ENABLED;
     g_nodeConfigSettings.userlan_monitor_port = DEFAULT_ULAN_MON_LINK;
+
+    //ignore_missing_calib_data
+    g_nodeConfigSettings.ignore_missing_calib_data = DEFAULT_IGNORE_MISSING_CALIB;
 }
 
 void Ros_ConfigFile_CheckYamlEvent(yaml_event_t* event)
@@ -753,6 +757,7 @@ void Ros_ConfigFile_PrintActiveConfiguration(Ros_Configuration_Settings const* c
     Ros_Debug_BroadcastMsg("Config: allow_custom_inform_job = %d", config->allow_custom_inform_job);
     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);
 }
 
 void Ros_ConfigFile_Parse()

src/ConfigFile.h

@@ -97,6 +97,8 @@ typedef enum
 #define DEFAULT_ULAN_MON_ENABLED        TRUE
 #define DEFAULT_ULAN_MON_LINK           CFG_ROS_USER_LAN_AUTO
 
+#define DEFAULT_IGNORE_MISSING_CALIB    FALSE
+
 typedef struct
 {
     //TODO(gavanderhoorn): add support for unsigned types
@@ -137,6 +139,8 @@ typedef struct
 
     BOOL userlan_monitor_enabled;
     Ros_UserLan_Port_Setting userlan_monitor_port;
+
+    BOOL ignore_missing_calib_data;
 } Ros_Configuration_Settings;
 
 extern Ros_Configuration_Settings g_nodeConfigSettings;

src/ControllerStatusIO.c

@@ -11,6 +11,9 @@ Controller g_Ros_Controller;
 ControllerStatus_Publishers g_publishers_RobotStatus;
 ControllerStatus_Messages g_messages_RobotStatus;
 
+//'private' APIs
+static BOOL Ros_Controller_LoadGroupCalibrationData(Controller* const controller);
+
 //-------------------------------------------------------------------
 // Wait for the controller to be ready to start initialization
 //-------------------------------------------------------------------
@@ -34,7 +37,6 @@ BOOL Ros_Controller_WaitInitReady()
 BOOL Ros_Controller_Initialize()
 {
     int groupIndex;
-    int i;
     BOOL bInitOk;
     STATUS status;
 
@@ -114,23 +116,29 @@ BOOL Ros_Controller_Initialize()
     }
 
     //get the robot calibration data for multi-robot systems
-    for (i = 0; i < MAX_ROBOT_CALIBRATION_FILES; i += 1)
+    const BOOL bCalibLoadedOk = Ros_Controller_LoadGroupCalibrationData(&g_Ros_Controller);
+    //see whether the user should be notified about failures (it's OK to not
+    //have/load any calibration in some cases)
+    const BOOL bNeedToWarnAboutCalib = Ros_Controller_ShouldWarnNoCalibDataLoaded(
+        &g_Ros_Controller, bCalibLoadedOk, g_nodeConfigSettings.publish_tf);
+    Ros_Debug_BroadcastMsg(
+        "%s: calib loaded ok: %s, should warn: %s", __func__,
+            bCalibLoadedOk ? "yes" : "no", bNeedToWarnAboutCalib ? "yes" : "no");
+    if (bNeedToWarnAboutCalib)
     {
-        MP_RB_CALIB_DATA calibData;
-        if (Ros_mpGetRobotCalibrationData(i, &calibData) == OK)
+        if (g_nodeConfigSettings.ignore_missing_calib_data)
         {
-            if (calibData.s_rb.grp_no <= MP_R8_GID && //the slave is a robot
-                calibData.m_rb.grp_no <= MP_R8_GID) //the master is another robot's RF
-            {
-                groupIndex = mpCtrlGrpId2GrpNo((MP_GRP_ID_TYPE)calibData.s_rb.grp_no);
-                MP_COORD* coord = &g_Ros_Controller.ctrlGroups[groupIndex]->robotCalibrationToBaseFrame;
-                coord->x = calibData.pos_uow[0];
-                coord->y = calibData.pos_uow[1];
-                coord->z = calibData.pos_uow[2];
-                coord->rx = calibData.ang_uow[0];
-                coord->ry = calibData.ang_uow[1];
-                coord->rz = calibData.ang_uow[2];
-            }
+            Ros_Debug_BroadcastMsg(
+                "%s: ignoring absence of calibration data (or load failure) as configured",
+                __func__);
+        }
+        else
+        {
+            mpSetAlarm(ALARM_CONFIGURATION_FAIL, "No calibration: invalid TF",
+                SUBCODE_CONFIGURATION_NO_CALIB_FILES_LOADED);
+            Ros_Debug_BroadcastMsg(
+                "%s: no calibration files loaded: TF potentially incorrect, posting warning "
+                "(disable with 'ignore_missing_calib_data')", __func__);
         }
     }
 
@@ -761,3 +769,82 @@ int Ros_Controller_GetActiveAlarmCodes(USHORT active_alarms[MAX_ALARM_COUNT + MA
 
     return num_entries;
 }
+
+/**
+ * Attempt to load on-controller extrinsic kinematic calibration data to improve
+ * accuracy of TF broadcasts (for frames for which such calibration data exists).
+ *
+ * NOTE: returns TRUE IFF at least one calibration file loaded successfully,
+ *       FALSE in all other cases.
+*/
+static BOOL Ros_Controller_LoadGroupCalibrationData(Controller* const controller)
+{
+    BOOL bOneCalibFileLoaded = FALSE;
+
+    for (int i = 0; i < MAX_ROBOT_CALIBRATION_FILES; i += 1)
+    {
+        MP_RB_CALIB_DATA calibData;
+        if (Ros_mpGetRobotCalibrationData(i, &calibData) == OK)
+        {
+            Ros_Debug_BroadcastMsg("%s: file %d loaded OK", __func__, i);
+
+            //Take note at least one calibration file loaded successfully.
+            //
+            //NOTE: successful loading of a single calibration file will of
+            //course not mean all groups (that would benefit from it) are actually
+            //calibrated correctly. But this is the best we can do, as there is
+            //no requirement to calibrate every group to every other group, nor
+            //will this prevent MotoROS2 from successfully controlling motion.
+            //
+            //TF frame broadcasts for uncalibrated groups will however be incorrect,
+            //which is why posting an alarm in case there are no calibration files
+            //at all on a multigroup system is considered a good thing to do.
+            bOneCalibFileLoaded = TRUE;
+
+            if (calibData.s_rb.grp_no <= MP_R8_GID && //the slave is a robot
+                calibData.m_rb.grp_no <= MP_R8_GID) //the master is another robot's RF
+            {
+                int groupIndex = mpCtrlGrpId2GrpNo((MP_GRP_ID_TYPE)calibData.s_rb.grp_no);
+                MP_COORD* coord = &controller->ctrlGroups[groupIndex]->robotCalibrationToBaseFrame;
+                coord->x = calibData.pos_uow[0];
+                coord->y = calibData.pos_uow[1];
+                coord->z = calibData.pos_uow[2];
+                coord->rx = calibData.ang_uow[0];
+                coord->ry = calibData.ang_uow[1];
+                coord->rz = calibData.ang_uow[2];
+            }
+        }
+    }
+
+    return bOneCalibFileLoaded;
+}
+
+//TODO(gavanderhoorn): should be able to make this 'static' once Yaskawa-Global/motoros2#173
+//is implemented and we can unit test static functions
+BOOL Ros_Controller_ShouldWarnNoCalibDataLoaded(Controller const* controller, BOOL bCalibLoadedOk, BOOL bPublishTfEnabled)
+{
+    size_t numRobotsOrStations = 0;
+    for(size_t i=0; i < controller->numGroup; ++i)
+        if (Ros_CtrlGroup_IsRobot(controller->ctrlGroups[i]) || Ros_CtrlGroup_IsStation(controller->ctrlGroups[i]))
+            numRobotsOrStations += 1;
+
+    //It's OK if calibration did not load (for single-group systems fi, or systems
+    //that haven't been calibrated), but it might be prudent to post an alarm just
+    //in case IFF:
+    //
+    // - this is a multi-group system
+    // - there is at least one robot
+    // - there are groups other than base groups (so R1+R2, or R1+S1)
+    // - TF broadcast is enabled, and
+    // - none of the calibration files loaded
+    //
+    //in that case there could be Robot groups that have not been calibrated (to
+    //either other robots and/or Station groups) and this would make their TF
+    //broadcasts incorrect.
+    return (
+        //only groups other than base groups need calibration
+        numRobotsOrStations > 1 &&
+        //and warn only if TF is enabled AND calib failed to load
+        TRUE  == bPublishTfEnabled &&
+        FALSE == bCalibLoadedOk);
+}

src/ControllerStatusIO.h

@@ -144,6 +144,8 @@ extern int Ros_Controller_GetAlarmCode();
 //retrieve all active alarms (and a possible error) and store them in 'active_alarms'
 extern int Ros_Controller_GetActiveAlarmCodes(USHORT active_alarms[MAX_ALARM_COUNT + MAX_ERROR_COUNT]);
 
+//TODO(gavanderhoorn): make static, see comment on definition
+extern BOOL Ros_Controller_ShouldWarnNoCalibDataLoaded(Controller const* controller, BOOL bCalibLoadedOk, BOOL bPublishTfEnabled);
 
 //#define DUMMY_SERVO_MODE 1    // Dummy servo mode is used for testing with Yaskawa debug controllers
 #ifdef DUMMY_SERVO_MODE

src/CtrlGroup.c

@@ -724,6 +724,22 @@ BOOL Ros_CtrlGroup_IsRobot(CtrlGroup* ctrlGroup)
     return((ctrlGroup->groupId >= MP_R1_GID) && (ctrlGroup->groupId <= MP_R8_GID));
 }
 
+//-------------------------------------------------------------------
+// Returns TRUE if the specified group is defined as a base
+//-------------------------------------------------------------------
+BOOL Ros_CtrlGroup_IsBase(CtrlGroup const* const ctrlGroup)
+{
+    return ((ctrlGroup->groupId >= MP_B1_GID) && (ctrlGroup->groupId <= MP_B8_GID));
+}
+
+//-------------------------------------------------------------------
+// Returns TRUE if the specified group is defined as a station
+//-------------------------------------------------------------------
+BOOL Ros_CtrlGroup_IsStation(CtrlGroup const* const ctrlGroup)
+{
+    return ((ctrlGroup->groupId >= MP_S1_GID) && (ctrlGroup->groupId <= MP_S24_GID));
+}
+
 //-------------------------------------------------------------------
 // Returns TRUE if the specified axis in the provided group is an
 // invalid one (ie: set to AXIS_INVALID)

src/CtrlGroup.h

@@ -129,6 +129,8 @@ extern void Ros_CtrlGroup_ConvertRosUnitsToMotoUnits(CtrlGroup* ctrlGroup, doubl
 extern UCHAR Ros_CtrlGroup_GetAxisConfig(CtrlGroup* ctrlGroup);
 
 extern BOOL Ros_CtrlGroup_IsRobot(CtrlGroup* ctrlGroup);
+extern BOOL Ros_CtrlGroup_IsBase(CtrlGroup const* const ctrlGroup);
+extern BOOL Ros_CtrlGroup_IsStation(CtrlGroup const* const ctrlGroup);
 extern BOOL Ros_CtrlGroup_IsInvalidAxis(CtrlGroup const* const ctrlGroup, size_t axisIdx);
 extern BOOL Ros_CtrlGroup_HasBaseTrack(CtrlGroup const* ctrlGroup);
 

src/ErrorHandling.h

@@ -196,6 +196,7 @@ typedef enum
     SUBCODE_CONFIGURATION_INVALID_USERLAN_MONITOR_PORT,
     SUBCODE_CONFIGURATION_USERLAN_MONITOR_AUTO_DETECT_FAILED,
     SUBCODE_CONFIGURATION_RUNTIME_USERLAN_LINKUP_ERR,
+    SUBCODE_CONFIGURATION_NO_CALIB_FILES_LOADED,
 } ALARM_CONFIGURATION_FAIL_SUBCODE; //8013
 
 typedef enum

src/MotoROS.h

@@ -101,6 +101,7 @@
 #include "Tests_CtrlGroup.h"
 #include "Tests_TestUtils.h"
 #include "Tests_RosMotoPlusConversionUtils.h"
+#include "Tests_ControllerStatusIO.h"
 #include "FauxCommandLineArgs.h"
 #include "InformCheckerAndGenerator.h"
 #include "MathConstants.h"

src/MotoROS2_AllControllers.vcxproj

@@ -343,6 +343,7 @@
     <ClCompile Include="ServiceStopTrajMode.c" />
     <ClCompile Include="ServiceStartTrajMode.c" />
     <ClCompile Include="ServiceSelectMotionTool.c" />
+    <ClCompile Include="Tests_ControllerStatusIO.c" />
     <ClCompile Include="Tests_CtrlGroup.c" />
     <ClCompile Include="Tests_TestUtils.c" />
     <ClCompile Include="Tests_RosMotoPlusConversionUtils.c" />
@@ -375,6 +376,7 @@
     <ClInclude Include="ServiceStopTrajMode.h" />
     <ClInclude Include="ServiceStartTrajMode.h" />
     <ClInclude Include="ServiceSelectMotionTool.h" />
+    <ClInclude Include="Tests_ControllerStatusIO.h" />
     <ClInclude Include="Tests_CtrlGroup.h" />
     <ClInclude Include="Tests_TestUtils.h" />
     <ClInclude Include="Tests_RosMotoPlusConversionUtils.h" />

src/MotoROS2_AllControllers.vcxproj.filters

@@ -303,6 +303,9 @@
     <ClCompile Include="ConfigFile.c">
       <Filter>Source Files\Parameters and Config</Filter>
     </ClCompile>
+    <ClCompile Include="Tests_ControllerStatusIO.c">
+      <Filter>Source Files\Tests</Filter>
+    </ClCompile>
     <ClCompile Include="Tests_CtrlGroup.c">
       <Filter>Source Files\Tests</Filter>
     </ClCompile>
@@ -344,6 +347,9 @@
     <ClInclude Include="CommunicationExecutor.h">
       <Filter>Header Files\Executors</Filter>
     </ClInclude>
+    <ClInclude Include="Tests_ControllerStatusIO.h">
+      <Filter>Header Files\Tests</Filter>
+    </ClInclude>
     <ClInclude Include="Tests_CtrlGroup.h">
       <Filter>Header Files\Tests</Filter>
     </ClInclude>