diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.cpp index 78ef92cb4b782954717c7d8ec4ba1f5ac91bcb00..5be64094faffdf6c00e4b542f5394f79c6ec1f79 100644 --- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.cpp +++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.cpp @@ -272,11 +272,12 @@ namespace armarx { rtGetThreadTimingsSensorDevice().rtMarkRtHandleInvalidTargetsStart(); bool oneIsInvalid = 0; - for (std::size_t i = 0; i < rtGetControlDevices().size(); ++i) + const auto& cdevs = rtGetControlDevices(); + for (std::size_t i = 0; i < cdevs.size(); ++i) { if (!rtGetActivatedJointControllers().at(i)->rtIsTargetVaild()) { - ARMARX_ERROR << "INVALID TARGET: " << rtGetControlDevices().at(i)->getDeviceName(); + ARMARX_RT_LOGF_ERROR("INVALID TARGET for JointController (idx = %lu) '%s'", i, cdevs.at(i)->rtGetDeviceName().c_str()); rtDeactivateAssignedNJointControllerBecauseOfError(i, false); oneIsInvalid = true; } @@ -322,6 +323,10 @@ namespace armarx { if (nJointCtrl->rtGetErrorState()) { + ARMARX_RT_LOGF_ERROR( + "NJointController '%s' requested deactivation while activating it", + nJointCtrl->rtGetInstanceName().c_str() + ); rtDeactivateNJointControllerBecauseOfError(nJointCtrlIndex, false); activeControllersChanged = true; } @@ -332,16 +337,28 @@ namespace armarx auto duration = TimeUtil::GetTime(true) - start; if (nJointCtrl->rtGetErrorState()) { + ARMARX_RT_LOGF_ERROR( + "NJointController '%s' requested deactivation while running it", + nJointCtrl->rtGetInstanceName().c_str() + ); 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!"; + ARMARX_RT_LOGF_ERROR( + "The NJointController '%s' took %ld µs to run!", + nJointCtrl->rtGetInstanceName().c_str(), + duration.toMicroSeconds() + ).deactivateSpam(5); } 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!"; + ARMARX_RT_LOGF_WARNING( + "The NJointController '%s' took %ld µs to run!", + nJointCtrl->rtGetInstanceName().c_str(), + duration.toMicroSeconds() + ).deactivateSpam(5); } } } @@ -434,7 +451,7 @@ namespace armarx ": no NJoint controller controls this device (name = " << rtGetControlDevices().at(ctrlDevIndex)->getDeviceName() << ", ControlMode = " << rtGetActivatedJointControllers().at(ctrlDevIndex)->getControlMode() << ")!" << "\n" - << VAROUT(ctrlDevIndex) << "\n" + << VAROUT(ctrlDevIndex) << "\nrtGetActivatedJointControllers\n" << ARMARX_STREAM_PRINTER { for (std::size_t i = 0; i < rtGetActivatedJointControllers().size(); ++i) @@ -590,6 +607,7 @@ namespace armarx void ControlThread::rtCommitActivatedControllers() { + ARMARX_RT_LOGF_DEBUG("RT: activated controllers changed"); ControlThreadDataBufferAttorneyForControlThread::CommitActivatedControllers(this); }