diff --git a/src/MotoROS.h b/src/MotoROS.h
index 6e6a910c..2e587e5b 100644
--- 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
index 504aabfa..7e3d6c01 100644
--- a/src/MotoROS2_AllControllers.vcxproj
+++ b/src/MotoROS2_AllControllers.vcxproj
@@ -343,6 +343,7 @@
+
@@ -375,6 +376,7 @@
+
diff --git a/src/MotoROS2_AllControllers.vcxproj.filters b/src/MotoROS2_AllControllers.vcxproj.filters
index 977338da..96571cdc 100644
--- a/src/MotoROS2_AllControllers.vcxproj.filters
+++ b/src/MotoROS2_AllControllers.vcxproj.filters
@@ -303,6 +303,9 @@
Source Files\Parameters and Config
+
+ Source Files\Tests
+
Source Files\Tests
@@ -344,6 +347,9 @@
Header Files\Executors
+
+ Header Files\Tests
+
Header Files\Tests
diff --git a/src/Tests_ControllerStatusIO.c b/src/Tests_ControllerStatusIO.c
new file mode 100644
index 00000000..15a92134
--- /dev/null
+++ 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
new file mode 100644
index 00000000..cfc681d9
--- /dev/null
+++ 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
index dcf82d3a..bd65d1a6 100644
--- 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