From 3a65f44bd94d49013aa70a520f9a89a50c08e1e8 Mon Sep 17 00:00:00 2001
From: Raphael Grimm <raphael.grimm@kit.edu>
Date: Wed, 25 Jul 2018 16:41:26 +0200
Subject: [PATCH] Check for NJointController error state
---
.../RobotUnitModuleControlThread.cpp | 18 +++++++++++++++++-
1 file changed, 17 insertions(+), 1 deletion(-)
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.cpp
index 34057ef95..7b411b54c 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.cpp
@@ -312,6 +312,7 @@ namespace armarx
void ControlThread::rtRunNJointControllers(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
{
rtGetThreadTimingsSensorDevice().rtMarkRtRunNJointControllersStart();
+ bool activeControllersChanged = false;
for (std::size_t nJointCtrlIndex = 0; nJointCtrlIndex < rtGetActivatedNJointControllers().size(); ++nJointCtrlIndex)
{
NJointController* nJointCtrl = rtGetActivatedNJointControllers().at(nJointCtrlIndex);
@@ -319,10 +320,21 @@ namespace armarx
{
if (nJointCtrl)
{
+ if(nJointCtrl->rtGetErrorState())
+ {
+ rtDeactivateNJointControllerBecauseOfError(nJointCtrlIndex, false);
+ activeControllersChanged = true;
+ }
+
auto start = TimeUtil::GetTime(true);
rtSyncNJointControllerRobot(nJointCtrl);
nJointCtrl->rtSwapBufferAndRun(sensorValuesTimestamp, timeSinceLastIteration);
auto duration = TimeUtil::GetTime(true) - start;
+ if(nJointCtrl->rtGetErrorState())
+ {
+ rtDeactivateNJointControllerBecauseOfError(nJointCtrlIndex, false);
+ activeControllersChanged = true;
+ }
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!";
@@ -339,9 +351,13 @@ namespace armarx
<< " threw an exception and is now deactivated: "
<< GetHandledExceptionString();
rtDeactivateNJointControllerBecauseOfError(nJointCtrlIndex, false);
+ activeControllersChanged = true;
}
}
- rtCommitActivatedControllers();
+ if(activeControllersChanged)
+ {
+ rtCommitActivatedControllers();
+ }
rtGetThreadTimingsSensorDevice().rtMarkRtRunNJointControllersEnd();
}
--
GitLab