From 00692e99655c271cb9725dfe1b2d0ad5d63155d8 Mon Sep 17 00:00:00 2001
From: ARMAR-7 User <armar7a@kit.edu>
Date: Tue, 10 Oct 2023 09:09:38 +0200
Subject: [PATCH] RobotUnitModuleSelfCollisionChecker: using metronome. adding
warning
---
.../RobotUnitModuleSelfCollisionChecker.cpp | 18 ++++++++++++++----
1 file changed, 14 insertions(+), 4 deletions(-)
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleSelfCollisionChecker.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleSelfCollisionChecker.cpp
index 2423d3b13..cea59a108 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.";
+ }
}
}
--
GitLab