Skip to content

Commit

Permalink
Raise alarm in case none of the calib files loaded
Browse files Browse the repository at this point in the history
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.
  • Loading branch information
gavanderhoorn committed Oct 3, 2023
1 parent 4d172e1 commit c640769
Show file tree
Hide file tree
Showing 2 changed files with 34 additions and 2 deletions.
35 changes: 33 additions & 2 deletions src/ControllerStatusIO.c
Original file line number Diff line number Diff line change
Expand Up @@ -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
{
Expand All @@ -52,7 +70,8 @@ static BOOL Ros_Controller_LoadGroupCalibrationData(Controller* const controller
}
}
}
return TRUE;

return bOneCalibFileLoaded;
}

//-------------------------------------------------------------------
Expand Down Expand Up @@ -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);
Expand Down
1 change: 1 addition & 0 deletions src/ErrorHandling.h
Original file line number Diff line number Diff line change
Expand Up @@ -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,

Check failure on line 197 in src/ErrorHandling.h

View workflow job for this annotation

GitHub Actions / lint

no documentation for '8013[16]' in 'doc/troubleshooting.md'
} ALARM_CONFIGURATION_FAIL_SUBCODE; //8013

typedef enum
Expand Down

0 comments on commit c640769

Please sign in to comment.