diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleSelfCollisionChecker.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleSelfCollisionChecker.cpp index 2423d3b13204734a7ff3921ea995d8ceb0bf4df6..cea59a108be1341b6f48c49e0d158682005d88ec 100644 --- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleSelfCollisionChecker.cpp +++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleSelfCollisionChecker.cpp @@ -31,6 +31,7 @@ #include <VirtualRobot/RobotNodeSet.h> #include "ArmarXCore/core/logging/Logging.h" +#include "ArmarXCore/core/time/Metronome.h" #include <ArmarXCore/core/util/OnScopeExit.h> #include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerBase.h> @@ -363,13 +364,15 @@ namespace armarx::RobotUnitModule }; while (true) { - const auto startT = std::chrono::high_resolution_clock::now(); + const auto freq = checkFrequency.load(); + + core::time::Metronome metronome(Frequency::Hertz(freq)); + //done if (isShuttingDown()) { return; } - const auto freq = checkFrequency.load(); const bool inEmergencyStop = _module<ControlThread>().getEmergencyStopState() == eEmergencyStopActive; if (inEmergencyStop || freq == 0) @@ -435,9 +438,16 @@ namespace armarx::RobotUnitModule << nodePairsToCheck.size() << " pairs"; } } + //sleep remaining - std::this_thread::sleep_until( - startT + std::chrono::microseconds{static_cast<int64_t>(1000000 / freq)}); + const auto duration = metronome.waitForNextTick(); + + if(not duration.isPositive()) + { + ARMARX_WARNING << deactivateSpam(10) << + "Self collision checking took too long. " + "Exceeding time budget by " << duration.toMilliSecondsDouble() << "ms."; + } } }