From 69fdf55881edd082992215f4fcb50bd1073c0d69 Mon Sep 17 00:00:00 2001 From: gavanderhoorn Date: Tue, 24 Oct 2023 14:46:32 +0200 Subject: [PATCH] Add tests for ShouldWarnNoCalibDataLoaded(..) Initial set to encode assumptions. --- src/MotoROS.h | 1 + src/Tests_ControllerStatusIO.c | 329 +++++++++++++++++++++++++++++++++ src/Tests_ControllerStatusIO.h | 17 ++ src/main.c | 1 + 4 files changed, 348 insertions(+) create mode 100644 src/Tests_ControllerStatusIO.c create mode 100644 src/Tests_ControllerStatusIO.h 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/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