Add tests for ShouldWarnNoCalibDataLoaded(..)

Initial set to encode assumptions.
Yaskawa-Global · Nov 23, 2023 · 0ea03c5 · 0ea03c5
1 parent 8a6049d
commit 0ea03c5
Show file tree

Hide file tree

Showing 6 changed files with 356 additions and 0 deletions.
diff --git a/src/MotoROS.h b/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"

diff --git a/src/MotoROS2_AllControllers.vcxproj b/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" />

diff --git a/src/MotoROS2_AllControllers.vcxproj.filters b/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>

diff --git a/src/Tests_ControllerStatusIO.c b/src/Tests_ControllerStatusIO.c
@@ -0,0 +1,329 @@
+// Tests_ControllerStatusIO.c
+
+// SPDX-FileCopyrightText: 2023, Yaskawa America, Inc.
+// SPDX-FileCopyrightText: 2023, Delft University of Technology
+//
+// SPDX-License-Identifier: Apache-2.0
+
+#ifdef MOTOROS2_TESTING_ENABLE
+
+#include "MotoROS.h"
+
+void Ros_Testing_ControllerStatusIO_MakeFake6dofRobot(CtrlGroup* group, int groupNo, MP_GRP_ID_TYPE groupId)
+{
+    bzero(group, sizeof(CtrlGroup));
+
+    group->groupNo = groupNo;
+    group->groupId = groupId;
+    group->numAxes = 6;
+
+    group->baseTrackGroupId = (MP_GRP_ID_TYPE)-1;
+    group->baseTrackGroupIndex = -1;
+
+    group->axisType.type[0] = AXIS_ROTATION; //s
+    group->axisType.type[1] = AXIS_ROTATION; //l
+    group->axisType.type[2] = AXIS_ROTATION; //u
+    group->axisType.type[3] = AXIS_ROTATION; //r
+    group->axisType.type[4] = AXIS_ROTATION; //b
+    group->axisType.type[5] = AXIS_ROTATION; //t
+    group->axisType.type[6] = AXIS_INVALID;
+    group->axisType.type[7] = AXIS_INVALID;
+}
+
+void Ros_Testing_ControllerStatusIO_MakeFakeBaseGroup(CtrlGroup* group, int groupNo, MP_GRP_ID_TYPE groupId)
+{
+    bzero(group, sizeof(CtrlGroup));
+
+    group->groupNo = groupNo;
+    group->groupId = groupId;
+    group->numAxes = 1;
+
+    group->baseTrackGroupId = (MP_GRP_ID_TYPE)-1;
+    group->baseTrackGroupIndex = -1;
+
+    group->axisType.type[0] = AXIS_LINEAR; //x
+    group->axisType.type[1] = AXIS_INVALID;
+    group->axisType.type[2] = AXIS_INVALID;
+    group->axisType.type[3] = AXIS_INVALID;
+    group->axisType.type[4] = AXIS_INVALID;
+    group->axisType.type[5] = AXIS_INVALID;
+    group->axisType.type[6] = AXIS_INVALID;
+    group->axisType.type[7] = AXIS_INVALID;
+}
+
+void Ros_Testing_ControllerStatusIO_MakeFakeStationGroup(CtrlGroup* group, int groupNo, MP_GRP_ID_TYPE groupId)
+{
+    bzero(group, sizeof(CtrlGroup));
+
+    group->groupNo = groupNo;
+    group->groupId = groupId;
+    group->numAxes = 1;
+
+    group->baseTrackGroupId = (MP_GRP_ID_TYPE)-1;
+    group->baseTrackGroupIndex = -1;
+
+    group->axisType.type[0] = AXIS_ROTATION;
+    group->axisType.type[1] = AXIS_INVALID;
+    group->axisType.type[2] = AXIS_INVALID;
+    group->axisType.type[3] = AXIS_INVALID;
+    group->axisType.type[4] = AXIS_INVALID;
+    group->axisType.type[5] = AXIS_INVALID;
+    group->axisType.type[6] = AXIS_INVALID;
+    group->axisType.type[7] = AXIS_INVALID;
+}
+
+void Ros_Testing_ControllerStatusIO_AssignRobotToBaseGroup(CtrlGroup* robot, CtrlGroup* base)
+{
+    robot->baseTrackGroupId = base->groupId;
+    robot->baseTrackGroupIndex = base->groupNo;
+}
+
+BOOL Ros_Testing_ControllerStatusIO_ShouldWarnNoCalibDataLoaded_R1()
+{
+    BOOL bSuccess = TRUE;
+
+    Controller controller;
+    CtrlGroup grp0;
+
+    controller.numGroup = 1;
+    controller.numRobot = 1;
+    controller.ctrlGroups[0] = &grp0;
+
+    Ros_Testing_ControllerStatusIO_MakeFake6dofRobot(&grp0, /*groupNo=*/ 0, /*groupId=*/ MP_R1_GID);
+
+    //R1: NO warning, nothing to calibrate with a single group
+    bSuccess &= FALSE == Ros_Controller_ShouldWarnNoCalibDataLoaded(&controller, /*bCalibLoadedOk=*/ FALSE, /*bPublishTfEnabled=*/ FALSE);
+    bSuccess &= FALSE == Ros_Controller_ShouldWarnNoCalibDataLoaded(&controller, /*bCalibLoadedOk=*/ TRUE , /*bPublishTfEnabled=*/ FALSE);
+    bSuccess &= FALSE == Ros_Controller_ShouldWarnNoCalibDataLoaded(&controller, /*bCalibLoadedOk=*/ FALSE, /*bPublishTfEnabled=*/ TRUE);
+    bSuccess &= FALSE == Ros_Controller_ShouldWarnNoCalibDataLoaded(&controller, /*bCalibLoadedOk=*/ TRUE , /*bPublishTfEnabled=*/ TRUE);
+
+    //report overall result
+    Ros_Debug_BroadcastMsg("Testing Ros_Testing_ControllerStatusIO_ShouldWarnNoCalibDataLoaded_R1: %s", bSuccess ? "PASS" : "FAIL");
+
+    return bSuccess;
+}
+
+BOOL Ros_Testing_ControllerStatusIO_ShouldWarnNoCalibDataLoaded_R1B1()
+{
+    BOOL bSuccess = TRUE;
+
+    Controller controller;
+    CtrlGroup grp0, grp1;
+
+    controller.numGroup = 2;
+    controller.numRobot = 1;
+    controller.ctrlGroups[0] = &grp0;
+    controller.ctrlGroups[1] = &grp1;
+
+    Ros_Testing_ControllerStatusIO_MakeFake6dofRobot(&grp0, /*groupNo=*/ 0, /*groupId=*/ MP_R1_GID);
+    Ros_Testing_ControllerStatusIO_MakeFakeBaseGroup(&grp1, /*groupNo=*/ 1, /*groupId=*/ MP_B1_GID);
+    Ros_Testing_ControllerStatusIO_AssignRobotToBaseGroup(/*robot=*/ &grp0, /*base=*/ &grp1);
+
+    //R1+B1: NO warning, as base tracks don't get calibrated
+    bSuccess &= FALSE == Ros_Controller_ShouldWarnNoCalibDataLoaded(&controller, /*bCalibLoadedOk=*/ FALSE, /*bPublishTfEnabled=*/ FALSE);
+    bSuccess &= FALSE == Ros_Controller_ShouldWarnNoCalibDataLoaded(&controller, /*bCalibLoadedOk=*/ TRUE , /*bPublishTfEnabled=*/ FALSE);
+    bSuccess &= FALSE == Ros_Controller_ShouldWarnNoCalibDataLoaded(&controller, /*bCalibLoadedOk=*/ FALSE, /*bPublishTfEnabled=*/ TRUE);
+    bSuccess &= FALSE == Ros_Controller_ShouldWarnNoCalibDataLoaded(&controller, /*bCalibLoadedOk=*/ TRUE , /*bPublishTfEnabled=*/ TRUE);
+
+    //report overall result
+    Ros_Debug_BroadcastMsg("Testing Ros_Testing_ControllerStatusIO_ShouldWarnNoCalibDataLoaded_R1B1: %s", bSuccess ? "PASS" : "FAIL");
+
+    return bSuccess;
+}
+
+BOOL Ros_Testing_ControllerStatusIO_ShouldWarnNoCalibDataLoaded_R1R2()
+{
+    BOOL bSuccess = TRUE;
+
+    Controller controller;
+    CtrlGroup grp0, grp1;
+
+    controller.numGroup = 2;
+    controller.numRobot = 1;
+    controller.ctrlGroups[0] = &grp0;
+    controller.ctrlGroups[1] = &grp1;
+
+    Ros_Testing_ControllerStatusIO_MakeFake6dofRobot(&grp0, /*groupNo=*/ 0, /*groupId=*/ MP_R1_GID);
+    Ros_Testing_ControllerStatusIO_MakeFake6dofRobot(&grp1, /*groupNo=*/ 1, /*groupId=*/ MP_R2_GID);
+
+    //R1+R2: YES warning, but only if TF enabled AND calib failed to load
+    bSuccess &= FALSE == Ros_Controller_ShouldWarnNoCalibDataLoaded(&controller, /*bCalibLoadedOk=*/ FALSE, /*bPublishTfEnabled=*/ FALSE);
+    bSuccess &= FALSE == Ros_Controller_ShouldWarnNoCalibDataLoaded(&controller, /*bCalibLoadedOk=*/ TRUE , /*bPublishTfEnabled=*/ FALSE);
+    bSuccess &= TRUE  == Ros_Controller_ShouldWarnNoCalibDataLoaded(&controller, /*bCalibLoadedOk=*/ FALSE, /*bPublishTfEnabled=*/ TRUE);
+    bSuccess &= FALSE == Ros_Controller_ShouldWarnNoCalibDataLoaded(&controller, /*bCalibLoadedOk=*/ TRUE , /*bPublishTfEnabled=*/ TRUE);
+
+    //report overall result
+    Ros_Debug_BroadcastMsg("Testing Ros_Testing_ControllerStatusIO_ShouldWarnNoCalibDataLoaded_R1R2: %s", bSuccess ? "PASS" : "FAIL");
+
+    return bSuccess;
+}
+
+BOOL Ros_Testing_ControllerStatusIO_ShouldWarnNoCalibDataLoaded_R1B1R2()
+{
+    BOOL bSuccess = TRUE;
+
+    Controller controller;
+    CtrlGroup grp0, grp1, grp2;
+
+    controller.numGroup = 3;
+    controller.numRobot = 2;
+    controller.ctrlGroups[0] = &grp0;
+    controller.ctrlGroups[1] = &grp1;
+    controller.ctrlGroups[2] = &grp2;
+
+    Ros_Testing_ControllerStatusIO_MakeFake6dofRobot(&grp0, /*groupNo=*/ 0, /*groupId=*/ MP_R1_GID);
+    Ros_Testing_ControllerStatusIO_MakeFakeBaseGroup(&grp2, /*groupNo=*/ 2, /*groupId=*/ MP_B1_GID);
+    Ros_Testing_ControllerStatusIO_AssignRobotToBaseGroup(/*robot=*/ &grp0, /*base=*/ &grp2);
+
+    Ros_Testing_ControllerStatusIO_MakeFake6dofRobot(&grp1, /*groupNo=*/ 1, /*groupId=*/ MP_R2_GID);
+
+    //R1B1+R2: YES warning, but only if TF enabled AND calib failed to load
+    bSuccess &= FALSE == Ros_Controller_ShouldWarnNoCalibDataLoaded(&controller, /*bCalibLoadedOk=*/ FALSE, /*bPublishTfEnabled=*/ FALSE);
+    bSuccess &= FALSE == Ros_Controller_ShouldWarnNoCalibDataLoaded(&controller, /*bCalibLoadedOk=*/ TRUE , /*bPublishTfEnabled=*/ FALSE);
+    bSuccess &= TRUE  == Ros_Controller_ShouldWarnNoCalibDataLoaded(&controller, /*bCalibLoadedOk=*/ FALSE, /*bPublishTfEnabled=*/ TRUE);
+    bSuccess &= FALSE == Ros_Controller_ShouldWarnNoCalibDataLoaded(&controller, /*bCalibLoadedOk=*/ TRUE , /*bPublishTfEnabled=*/ TRUE);
+
+    //report overall result
+    Ros_Debug_BroadcastMsg("Testing Ros_Testing_ControllerStatusIO_ShouldWarnNoCalibDataLoaded_R1B1R2: %s", bSuccess ? "PASS" : "FAIL");
+
+    return bSuccess;
+}
+
+BOOL Ros_Testing_ControllerStatusIO_ShouldWarnNoCalibDataLoaded_R1S1()
+{
+    BOOL bSuccess = TRUE;
+
+    Controller controller;
+    CtrlGroup grp0, grp1;
+
+    controller.numGroup = 2;
+    controller.numRobot = 1;
+    controller.ctrlGroups[0] = &grp0;
+    controller.ctrlGroups[1] = &grp1;
+
+    Ros_Testing_ControllerStatusIO_MakeFake6dofRobot(&grp0, /*groupNo=*/ 0, /*groupId=*/ MP_R1_GID);
+    Ros_Testing_ControllerStatusIO_MakeFakeStationGroup(&grp1, /*groupNo=*/ 1, /*groupId=*/ MP_S1_GID);
+
+    //R1S1: YES warning, but only if TF enabled AND calib failed to load
+    bSuccess &= FALSE == Ros_Controller_ShouldWarnNoCalibDataLoaded(&controller, /*bCalibLoadedOk=*/ FALSE, /*bPublishTfEnabled=*/ FALSE);
+    bSuccess &= FALSE == Ros_Controller_ShouldWarnNoCalibDataLoaded(&controller, /*bCalibLoadedOk=*/ TRUE , /*bPublishTfEnabled=*/ FALSE);
+    bSuccess &= TRUE  == Ros_Controller_ShouldWarnNoCalibDataLoaded(&controller, /*bCalibLoadedOk=*/ FALSE, /*bPublishTfEnabled=*/ TRUE);
+    bSuccess &= FALSE == Ros_Controller_ShouldWarnNoCalibDataLoaded(&controller, /*bCalibLoadedOk=*/ TRUE , /*bPublishTfEnabled=*/ TRUE);
+
+    //report overall result
+    Ros_Debug_BroadcastMsg("Testing Ros_Testing_ControllerStatusIO_ShouldWarnNoCalibDataLoaded_R1S1: %s", bSuccess ? "PASS" : "FAIL");
+
+    return bSuccess;
+}
+
+BOOL Ros_Testing_ControllerStatusIO_ShouldWarnNoCalibDataLoaded_R1S1S2()
+{
+    BOOL bSuccess = TRUE;
+
+    Controller controller;
+    CtrlGroup grp0, grp1, grp2;
+
+    controller.numGroup = 3;
+    controller.numRobot = 1;
+    controller.ctrlGroups[0] = &grp0;
+    controller.ctrlGroups[1] = &grp1;
+    controller.ctrlGroups[2] = &grp2;
+
+    Ros_Testing_ControllerStatusIO_MakeFake6dofRobot(&grp0, /*groupNo=*/ 0, /*groupId=*/ MP_R1_GID);
+    Ros_Testing_ControllerStatusIO_MakeFakeStationGroup(&grp1, /*groupNo=*/ 1, /*groupId=*/ MP_S1_GID);
+    Ros_Testing_ControllerStatusIO_MakeFakeStationGroup(&grp2, /*groupNo=*/ 2, /*groupId=*/ MP_S2_GID);
+
+    //R1S1S2: YES warning, but only if TF enabled AND calib failed to load
+    bSuccess &= FALSE == Ros_Controller_ShouldWarnNoCalibDataLoaded(&controller, /*bCalibLoadedOk=*/ FALSE, /*bPublishTfEnabled=*/ FALSE);
+    bSuccess &= FALSE == Ros_Controller_ShouldWarnNoCalibDataLoaded(&controller, /*bCalibLoadedOk=*/ TRUE , /*bPublishTfEnabled=*/ FALSE);
+    bSuccess &= TRUE  == Ros_Controller_ShouldWarnNoCalibDataLoaded(&controller, /*bCalibLoadedOk=*/ FALSE, /*bPublishTfEnabled=*/ TRUE);
+    bSuccess &= FALSE == Ros_Controller_ShouldWarnNoCalibDataLoaded(&controller, /*bCalibLoadedOk=*/ TRUE , /*bPublishTfEnabled=*/ TRUE);
+
+    //report overall result
+    Ros_Debug_BroadcastMsg("Testing Ros_Testing_ControllerStatusIO_ShouldWarnNoCalibDataLoaded_R1S1S2: %s", bSuccess ? "PASS" : "FAIL");
+
+    return bSuccess;
+}
+
+BOOL Ros_Testing_ControllerStatusIO_ShouldWarnNoCalibDataLoaded_R1B1S1()
+{
+    BOOL bSuccess = TRUE;
+
+    Controller controller;
+    CtrlGroup grp0, grp1, grp2;
+
+    controller.numGroup = 3;
+    controller.numRobot = 1;
+    controller.ctrlGroups[0] = &grp0;
+    controller.ctrlGroups[1] = &grp1;
+    controller.ctrlGroups[2] = &grp2;
+
+    Ros_Testing_ControllerStatusIO_MakeFake6dofRobot(&grp0, /*groupNo=*/ 0, /*groupId=*/ MP_R1_GID);
+    Ros_Testing_ControllerStatusIO_MakeFakeBaseGroup(&grp1, /*groupNo=*/ 1, /*groupId=*/ MP_B1_GID);
+    Ros_Testing_ControllerStatusIO_AssignRobotToBaseGroup(/*robot=*/ &grp0, /*base=*/ &grp1);
+
+    Ros_Testing_ControllerStatusIO_MakeFakeStationGroup(&grp2, /*groupNo=*/ 2, /*groupId=*/ MP_S1_GID);
+
+    //R1B1S1: YES warning, but only if TF enabled AND calib failed to load
+    bSuccess &= FALSE == Ros_Controller_ShouldWarnNoCalibDataLoaded(&controller, /*bCalibLoadedOk=*/ FALSE, /*bPublishTfEnabled=*/ FALSE);
+    bSuccess &= FALSE == Ros_Controller_ShouldWarnNoCalibDataLoaded(&controller, /*bCalibLoadedOk=*/ TRUE , /*bPublishTfEnabled=*/ FALSE);
+    bSuccess &= TRUE  == Ros_Controller_ShouldWarnNoCalibDataLoaded(&controller, /*bCalibLoadedOk=*/ FALSE, /*bPublishTfEnabled=*/ TRUE);
+    bSuccess &= FALSE == Ros_Controller_ShouldWarnNoCalibDataLoaded(&controller, /*bCalibLoadedOk=*/ TRUE , /*bPublishTfEnabled=*/ TRUE);
+
+    //report overall result
+    Ros_Debug_BroadcastMsg("Testing Ros_Testing_ControllerStatusIO_ShouldWarnNoCalibDataLoaded_R1B1S1: %s", bSuccess ? "PASS" : "FAIL");
+
+    return bSuccess;
+}
+
+BOOL Ros_Testing_ControllerStatusIO_ShouldWarnNoCalibDataLoaded_R1B1R2B2()
+{
+    BOOL bSuccess = TRUE;
+
+    Controller controller;
+    CtrlGroup grp0, grp1, grp2, grp3;
+
+    controller.numGroup = 4;
+    controller.numRobot = 2;
+    controller.ctrlGroups[0] = &grp0;
+    controller.ctrlGroups[1] = &grp1;
+    controller.ctrlGroups[2] = &grp2;
+    controller.ctrlGroups[3] = &grp3;
+
+    Ros_Testing_ControllerStatusIO_MakeFake6dofRobot(&grp0, /*groupNo=*/ 0, /*groupId=*/ MP_R1_GID);
+    Ros_Testing_ControllerStatusIO_MakeFakeBaseGroup(&grp1, /*groupNo=*/ 1, /*groupId=*/ MP_B1_GID);
+    Ros_Testing_ControllerStatusIO_AssignRobotToBaseGroup(/*robot=*/ &grp0, /*base=*/ &grp1);
+
+    Ros_Testing_ControllerStatusIO_MakeFake6dofRobot(&grp2, /*groupNo=*/ 2, /*groupId=*/ MP_R2_GID);
+    Ros_Testing_ControllerStatusIO_MakeFakeBaseGroup(&grp3, /*groupNo=*/ 3, /*groupId=*/ MP_B2_GID);
+    Ros_Testing_ControllerStatusIO_AssignRobotToBaseGroup(/*robot=*/ &grp2, /*base=*/ &grp3);
+
+    //R1B1R2B2: YES warning, but only if TF enabled AND calib failed to load
+    bSuccess &= FALSE == Ros_Controller_ShouldWarnNoCalibDataLoaded(&controller, /*bCalibLoadedOk=*/ FALSE, /*bPublishTfEnabled=*/ FALSE);
+    bSuccess &= FALSE == Ros_Controller_ShouldWarnNoCalibDataLoaded(&controller, /*bCalibLoadedOk=*/ TRUE , /*bPublishTfEnabled=*/ FALSE);
+    bSuccess &= TRUE  == Ros_Controller_ShouldWarnNoCalibDataLoaded(&controller, /*bCalibLoadedOk=*/ FALSE, /*bPublishTfEnabled=*/ TRUE);
+    bSuccess &= FALSE == Ros_Controller_ShouldWarnNoCalibDataLoaded(&controller, /*bCalibLoadedOk=*/ TRUE , /*bPublishTfEnabled=*/ TRUE);
+
+    //report overall result
+    Ros_Debug_BroadcastMsg("Testing Ros_Testing_ControllerStatusIO_ShouldWarnNoCalibDataLoaded_R1B1R2B2: %s", bSuccess ? "PASS" : "FAIL");
+
+    return bSuccess;
+}
+
+BOOL Ros_Testing_ControllerStatusIO()
+{
+    BOOL bSuccess = TRUE;
+
+    bSuccess &= Ros_Testing_ControllerStatusIO_ShouldWarnNoCalibDataLoaded_R1();
+    bSuccess &= Ros_Testing_ControllerStatusIO_ShouldWarnNoCalibDataLoaded_R1B1();
+    bSuccess &= Ros_Testing_ControllerStatusIO_ShouldWarnNoCalibDataLoaded_R1R2();
+    bSuccess &= Ros_Testing_ControllerStatusIO_ShouldWarnNoCalibDataLoaded_R1B1R2();
+    bSuccess &= Ros_Testing_ControllerStatusIO_ShouldWarnNoCalibDataLoaded_R1S1();
+    bSuccess &= Ros_Testing_ControllerStatusIO_ShouldWarnNoCalibDataLoaded_R1S1S2();
+    bSuccess &= Ros_Testing_ControllerStatusIO_ShouldWarnNoCalibDataLoaded_R1B1S1();
+    bSuccess &= Ros_Testing_ControllerStatusIO_ShouldWarnNoCalibDataLoaded_R1B1R2B2();
+
+    return bSuccess;
+}
+
+#endif //MOTOROS2_TESTING_ENABLE
diff --git a/src/Tests_ControllerStatusIO.h b/src/Tests_ControllerStatusIO.h
@@ -0,0 +1,17 @@
+// Tests_ControllerStatusIO.h
+
+// SPDX-FileCopyrightText: 2023, Yaskawa America, Inc.
+// SPDX-FileCopyrightText: 2023, Delft University of Technology
+//
+// SPDX-License-Identifier: Apache-2.0
+
+#ifndef MOTOROS2_TESTS_CONTROLLER_STATUS_IO_H
+#define MOTOROS2_TESTS_CONTROLLER_STATUS_IO_H
+
+#ifdef MOTOROS2_TESTING_ENABLE
+
+extern BOOL Ros_Testing_ControllerStatusIO();
+
+#endif //MOTOROS2_TESTING_ENABLE
+
+#endif  // MOTOROS2_TESTS_CONTROLLER_STATUS_IO_H
diff --git a/src/main.c b/src/main.c
@@ -72,6 +72,7 @@ void RosInitTask()
     BOOL bTestResult = TRUE;
     bTestResult &= Ros_Testing_CtrlGroup();
     bTestResult &= Ros_Testing_RosMotoPlusConversionUtils();
+    bTestResult &= Ros_Testing_ControllerStatusIO();
     bTestResult ? Ros_Debug_BroadcastMsg("Testing SUCCESSFUL") : Ros_Debug_BroadcastMsg("!!! Testing FAILED !!!");
     Ros_Debug_BroadcastMsg("===");
 #endif