From c64076900677d33db0dcdc8d1e1d9cae20205540 Mon Sep 17 00:00:00 2001
From: gavanderhoorn <g.a.vanderhoorn@tudelft.nl>
Date: Tue, 3 Oct 2023 11:15:24 +0200
Subject: [PATCH] Raise alarm in case none of the calib files loaded

Note: as mentioned in the comments, it's perfectly OK for multi-group
systems to not have been calibrated. Whether or not calibration
succeeded to load is therefore not taken into account when determining
whether controller initialisation was successful or not (`bInitOk`).

But, in case TF broadcasts are enabled, uncalibrated groups
(specifically: robots) will have their origins coincident, which will
(most likely) not be representative of their physical configuration (ie:
in case those groups are different manipulators, those will not occupy
the same physical space).

In those cases an alarm is raised to notify the user of this potential
misconfiguration.

But only if the alarm hasn't been disabled by user configuration.
---
 src/ControllerStatusIO.c | 35 +++++++++++++++++++++++++++++++++--
 src/ErrorHandling.h      |  1 +
 2 files changed, 34 insertions(+), 2 deletions(-)

diff --git a/src/ControllerStatusIO.c b/src/ControllerStatusIO.c
index 9e5535b4..73846e8f 100644
--- a/src/ControllerStatusIO.c
+++ b/src/ControllerStatusIO.c
@@ -30,14 +30,32 @@ BOOL Ros_Controller_WaitInitReady()
 /**
  * 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)
         {
+            //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
             {
@@ -52,7 +70,8 @@ static BOOL Ros_Controller_LoadGroupCalibrationData(Controller* const controller
             }
         }
     }
-    return TRUE;
+
+    return bOneCalibFileLoaded;
 }
 
 //-------------------------------------------------------------------
@@ -141,7 +160,19 @@ BOOL Ros_Controller_Initialize()
     }
 
     //get the robot calibration data for multi-robot systems
-    bInitOk = Ros_Controller_LoadGroupCalibrationData(&g_Ros_Controller);
+    BOOL bCalibLoadedOk = Ros_Controller_LoadGroupCalibrationData(&g_Ros_Controller);
+    //It's OK if calibration did not load (for single-group systems fi, or systems
+    //that haven't been calibrated), but post an alarm just in case IFF:
+    if (g_Ros_Controller.numGroup > 1 &&                                 // this is a multi-group system
+        TRUE  == g_nodeConfigSettings.publish_tf &&                      // TF broadcast is enabled
+        FALSE == bCalibLoadedOk &&                                       // none of the calibration files loaded, and
+        FALSE == g_nodeConfigSettings.disable_missing_calib_warning)     // the warning has not been disabled
+    {
+        Ros_Debug_BroadcastMsg("%s: no calibration files loaded: TF potentially incorrect -> warn "
+            "(disable with 'disable_missing_calib_warning')", __func__);
+        mpSetAlarm(ALARM_CONFIGURATION_FAIL, "No calibration: TF may by wrong",
+            SUBCODE_CONFIGURATION_NO_CALIB_FILES_LOADED);
+    }
 
 #ifdef DEBUG
     Ros_Debug_BroadcastMsg("g_Ros_Controller.numRobot = %d", g_Ros_Controller.numRobot);
diff --git a/src/ErrorHandling.h b/src/ErrorHandling.h
index f5e0ef8c..b3537c4a 100644
--- a/src/ErrorHandling.h
+++ b/src/ErrorHandling.h
@@ -194,6 +194,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