diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h index fec9d8df7e08598aefabe7c63ec8b90dd284d425..6085b8a6dbb29682eb1faec354fe195964449195 100644 --- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h +++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h @@ -714,6 +714,11 @@ namespace armarx */ bool rtGetErrorState() const; + /** + * @brief Returns the number of used \ref ControlDevice "ControlDevices" + * @return The number of used \ref ControlDevice "ControlDevices" + */ + std::size_t rtGetNumberOfUsedControlDevices() const; protected: /** * @brief This function is called before the controller is activated. diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.ipp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.ipp index 0cd310b2110adfe6d61191b10b6e7147dd07ffcb..a4b8ee87bc797d070aad620cc02b46a19f44caf8 100644 --- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.ipp +++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.ipp @@ -204,6 +204,12 @@ namespace armarx { return errorState; } + + inline std::size_t NJointController::rtGetNumberOfUsedControlDevices() const + { + return controlDeviceUsedIndices.size(); + } + //NJointControllerWithTripleBuffer<ControlDataStruct> template <typename ControlDataStruct> inline void NJointControllerWithTripleBuffer<ControlDataStruct>::rtSwapBufferAndRun( diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnit.h b/source/RobotAPI/components/units/RobotUnit/RobotUnit.h index 30541f3206fa56f344b77c524dac669af4b2cedb..eac584c039b03f9f01c5e819cecf9d585a0a61ee 100644 --- a/source/RobotAPI/components/units/RobotUnit/RobotUnit.h +++ b/source/RobotAPI/components/units/RobotUnit/RobotUnit.h @@ -40,6 +40,7 @@ namespace armarx public RobotUnitModule::RobotDataPropertyDefinitions, public RobotUnitModule::PublisherPropertyDefinitions, public RobotUnitModule::ManagementPropertyDefinitions, + public RobotUnitModule::ControlThreadPropertyDefinitions, public RobotUnitModule::SelfCollisionCheckerPropertyDefinitions { public: @@ -50,6 +51,7 @@ namespace armarx RobotDataPropertyDefinitions(prefix), PublisherPropertyDefinitions(prefix), ManagementPropertyDefinitions(prefix), + ControlThreadPropertyDefinitions(prefix), SelfCollisionCheckerPropertyDefinitions(prefix) {} }; diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.cpp index 811a2bf8789c636a18501d6800fed856e8030e22..3d5b227b8708243a7435ebb053908d9696df2a29 100644 --- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.cpp +++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.cpp @@ -323,11 +323,11 @@ namespace armarx rtSyncNJointControllerRobot(nJointCtrl); nJointCtrl->rtSwapBufferAndRun(sensorValuesTimestamp, timeSinceLastIteration); auto duration = TimeUtil::GetTime(true) - start; - if (duration.toMicroSeconds() > 500) + if (static_cast<std::size_t>(duration.toMicroSeconds()) > nJointCtrl->rtGetNumberOfUsedControlDevices() * usPerDevUntilError) { ARMARX_ERROR << deactivateSpam(5) << "The NJointController " << nJointCtrl->getClassName() << " took " << duration.toMicroSeconds() << " µs to run!"; } - else if (duration.toMicroSeconds() > 50) + else if (static_cast<std::size_t>(duration.toMicroSeconds()) > nJointCtrl->rtGetNumberOfUsedControlDevices() * usPerDevUntilWarn) { ARMARX_WARNING << deactivateSpam(5) << "The NJointController " << nJointCtrl->getClassName() << " took " << duration.toMicroSeconds() << " µs to run!"; } diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.h b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.h index 5d75c17c0c1a05785c0348fbea097683edf843de..37f33965001df47102dbf0a5349082fbd05f6f9e 100644 --- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.h +++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.h @@ -36,6 +36,24 @@ namespace armarx class RTThreadTimingsSensorDevice; namespace RobotUnitModule { + class ControlThreadPropertyDefinitions: public ModuleBasePropertyDefinitions + { + public: + ControlThreadPropertyDefinitions(std::string prefix): ModuleBasePropertyDefinitions(prefix) + { + defineOptionalProperty<std::size_t>( + "NjointController_AllowedExecutionTimePerControlDeviceUntilWarning", 2, + "A Warning will be printed, If the execution time in micro seconds of a NJointController " + "exceeds this parameter times the number of ControlDevices." + ); + defineOptionalProperty<std::size_t>( + "NjointController_AllowedExecutionTimePerControlDeviceUntilError", 20, + "An Error will be printed, If the execution time in micro seconds of a NJointController " + "exceeds this parameter times the number of ControlDevices." + ); + } + }; + /** * @brief This \ref ModuleBase "Module" manages the \ref ControlThread. * @@ -261,6 +279,11 @@ namespace armarx ///@brief The \ref ControlThread's thread id std::atomic<std::thread::id> controlThreadId; + + /// @brief A Warning will be printed, if the execution time per ControlDev of a NJointController exceeds this parameter + std::size_t usPerDevUntilWarn; + /// @brief An Error will be printed, if the execution time per ControlDev of a NJointController exceeds this parameter + std::size_t usPerDevUntilError; }; } }