From ebb49a2c900282bf45a315e7f8d2ae691aad92f4 Mon Sep 17 00:00:00 2001
From: Raphael Grimm <raphael.grimm@kit.edu>
Date: Wed, 9 May 2018 07:53:12 +0200
Subject: [PATCH] Add throwIfInControlThread to non rt function of RobotUnit
---
.../RobotUnitModuleControlThread.cpp | 13 +++--
...RobotUnitModuleControlThreadDataBuffer.cpp | 14 +++++
.../RobotUnitModuleControllerManagement.cpp | 46 ++++++++++++++++
.../RobotUnitModuleDevices.cpp | 54 ++++++++++---------
.../RobotUnitModuleLogging.cpp | 12 ++++-
.../RobotUnitModuleManagement.cpp | 1 +
.../RobotUnitModuleManagement.h | 1 +
.../RobotUnitModulePublisher.cpp | 18 +++++++
.../RobotUnitModuleRobotData.cpp | 9 +++-
.../RobotUnitModuleSelfCollisionChecker.cpp | 9 ++++
.../RobotUnitModules/RobotUnitModuleUnits.cpp | 18 ++++++-
.../RobotUnitModules/RobotUnitModuleUnits.ipp | 14 +++++
12 files changed, 174 insertions(+), 35 deletions(-)
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.cpp
index 775174e4d..e641b108b 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.cpp
@@ -257,7 +257,6 @@ namespace armarx
return true;
}
-
void ControlThread::rtResetAllTargets()
{
rtGetThreadTimingsSensorDevice().rtMarkRtResetAllTargetsStart();
@@ -268,7 +267,6 @@ namespace armarx
rtGetThreadTimingsSensorDevice().rtMarkRtResetAllTargetsEnd();
}
-
void ControlThread::rtHandleInvalidTargets()
{
rtGetThreadTimingsSensorDevice().rtMarkRtHandleInvalidTargetsStart();
@@ -288,7 +286,6 @@ namespace armarx
rtGetThreadTimingsSensorDevice().rtMarkRtHandleInvalidTargetsEnd();
}
-
void ControlThread::rtReadSensorDeviceValues(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
{
rtGetThreadTimingsSensorDevice().rtMarkRtReadSensorDeviceValuesStart();
@@ -300,7 +297,6 @@ namespace armarx
rtGetThreadTimingsSensorDevice().rtMarkRtReadSensorDeviceValuesEnd();
}
-
void ControlThread::rtRunJointControllers(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
{
rtGetThreadTimingsSensorDevice().rtMarkRtRunJointControllersStart();
@@ -311,7 +307,6 @@ namespace armarx
rtGetThreadTimingsSensorDevice().rtMarkRtRunJointControllersEnd();
}
-
void ControlThread::rtRunNJointControllers(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
{
rtGetThreadTimingsSensorDevice().rtMarkRtRunNJointControllersStart();
@@ -347,7 +342,6 @@ namespace armarx
rtGetThreadTimingsSensorDevice().rtMarkRtRunNJointControllersEnd();
}
-
void ControlThread::rtDeactivateNJointControllerBecauseOfError(std::size_t nJointCtrlIndex, bool writeActivatedControlers)
{
const NJointControllerPtr& nJointCtrl = rtGetActivatedNJointControllers().at(nJointCtrlIndex);
@@ -412,7 +406,6 @@ namespace armarx
}
}
-
void ControlThread::rtDeactivateAssignedNJointControllerBecauseOfError(std::size_t ctrlDevIndex, bool writeActivatedControlers)
{
std::size_t nJointCtrlIndex = rtGetActivatedJointToNJointControllerAssignement().at(ctrlDevIndex);
@@ -503,11 +496,13 @@ namespace armarx
void ControlThread::_preFinishControlThreadInitialization()
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
controlThreadId = std::this_thread::get_id();
}
void ControlThread::_preOnInitRobotUnit()
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
try
{
rtRobot = VirtualRobot::RobotIO::loadRobot(_module<RobotData>().getRobotFileName(), VirtualRobot::RobotIO::eStructure);
@@ -524,21 +519,25 @@ namespace armarx
void ControlThread::setEmergencyStopState(EmergencyStopState state, const Ice::Current&)
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
_module<Units>().getEmergencyStopMaster()->setEmergencyStopState(state);
}
EmergencyStopState ControlThread::getEmergencyStopState(const Ice::Current&) const
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
return emergencyStop ? EmergencyStopState::eEmergencyStopActive : EmergencyStopState::eEmergencyStopInactive;
}
EmergencyStopState ControlThread::getRtEmergencyStopState(const Ice::Current&) const
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
return rtIsInEmergencyStop() ? EmergencyStopState::eEmergencyStopActive : EmergencyStopState::eEmergencyStopInactive;
}
void ControlThread::setEmergencyStopStateNoReportToTopic(EmergencyStopState state)
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
emergencyStop = (state == EmergencyStopState::eEmergencyStopActive);
}
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThreadDataBuffer.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThreadDataBuffer.cpp
index b36eb85a3..0a33e1a88 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThreadDataBuffer.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThreadDataBuffer.cpp
@@ -65,6 +65,7 @@ namespace armarx
{
JointAndNJointControllers ControlThreadDataBuffer::getActivatedControllers() const
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
throwIfDevicesNotReady(__FUNCTION__);
std::lock_guard<std::recursive_mutex> guard {controllersActivatedMutex};
return controllersActivated.getReadBuffer();
@@ -72,6 +73,7 @@ namespace armarx
std::vector<JointController*> ControlThreadDataBuffer::getActivatedJointControllers() const
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
throwIfDevicesNotReady(__FUNCTION__);
std::lock_guard<std::recursive_mutex> guard {controllersActivatedMutex};
return controllersActivated.getReadBuffer().jointControllers;
@@ -79,12 +81,14 @@ namespace armarx
std::vector<NJointController*> ControlThreadDataBuffer::getActivatedNJointControllers() const
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
throwIfDevicesNotReady(__FUNCTION__);
std::lock_guard<std::recursive_mutex> guard {controllersActivatedMutex};
return controllersActivated.getReadBuffer().nJointControllers;
}
bool ControlThreadDataBuffer::activatedControllersChanged() const
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
throwIfDevicesNotReady(__FUNCTION__);
std::lock_guard<std::recursive_mutex> guard {controllersActivatedMutex};
return controllersActivated.updateReadBuffer();
@@ -92,6 +96,7 @@ namespace armarx
JointAndNJointControllers ControlThreadDataBuffer::copyRequestedControllers() const
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
throwIfDevicesNotReady(__FUNCTION__);
std::lock_guard<std::recursive_mutex> guard {controllersRequestedMutex};
return controllersRequested.getWriteBuffer();
@@ -99,6 +104,7 @@ namespace armarx
std::vector<JointController*> ControlThreadDataBuffer::copyRequestedJointControllers() const
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
throwIfDevicesNotReady(__FUNCTION__);
std::lock_guard<std::recursive_mutex> guard {controllersRequestedMutex};
return controllersRequested.getWriteBuffer().jointControllers;
@@ -106,6 +112,7 @@ namespace armarx
std::vector<NJointController*> ControlThreadDataBuffer::copyRequestedNJointControllers() const
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
throwIfDevicesNotReady(__FUNCTION__);
std::lock_guard<std::recursive_mutex> guard {controllersRequestedMutex};
return controllersRequested.getWriteBuffer().nJointControllers;
@@ -113,6 +120,7 @@ namespace armarx
bool ControlThreadDataBuffer::sensorAndControlBufferChanged() const
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
auto guard = getGuard();
throwIfDevicesNotReady(__FUNCTION__);
return controlThreadOutputBuffer.updateReadBuffer();
@@ -120,6 +128,7 @@ namespace armarx
const SensorAndControl& ControlThreadDataBuffer::getSensorAndControlBuffer() const
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
auto guard = getGuard();
throwIfDevicesNotReady(__FUNCTION__);
return controlThreadOutputBuffer.getReadBuffer();
@@ -127,6 +136,7 @@ namespace armarx
void ControlThreadDataBuffer::writeRequestedControllers(JointAndNJointControllers&& setOfControllers)
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
auto guard = getGuard();
throwIfDevicesNotReady(__FUNCTION__);
//check NJoint
@@ -224,6 +234,7 @@ namespace armarx
void ControlThreadDataBuffer::setActivateControllersRequest(std::set<NJointControllerPtr, std::greater<NJointControllerPtr>> ctrls)
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
auto guard = getGuard();
throwIfDevicesNotReady(__FUNCTION__);
//erase nullptr
@@ -289,15 +300,18 @@ namespace armarx
void ControlThreadDataBuffer::updateVirtualRobot(const VirtualRobot::RobotPtr& robot, const std::vector<VirtualRobot::RobotNodePtr>& nodes) const
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
_module<Devices>().updateVirtualRobotFromSensorValues(robot, nodes, controlThreadOutputBuffer.getReadBuffer().sensors);
}
void ControlThreadDataBuffer::updateVirtualRobot(const VirtualRobot::RobotPtr& robot) const
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
_module<Devices>().updateVirtualRobotFromSensorValues(robot, controlThreadOutputBuffer.getReadBuffer().sensors);
}
void ControlThreadDataBuffer::_postFinishDeviceInitialization()
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
ARMARX_DEBUG << "initializing buffers:";
{
ARMARX_DEBUG << "----initializing controller buffers for " << _module<Devices>().getNumberOfControlDevices() << " control devices";
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControllerManagement.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControllerManagement.cpp
index 3b5e3bb28..af602a752 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControllerManagement.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControllerManagement.cpp
@@ -65,18 +65,21 @@ namespace armarx
{
Ice::StringSeq ControllerManagement::getRequestedNJointControllerNames(const Ice::Current&) const
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
auto guard = getGuard();
return GetNonNullNames(_module<ControlThreadDataBuffer>().copyRequestedNJointControllers());
}
Ice::StringSeq ControllerManagement::getActivatedNJointControllerNames(const Ice::Current&) const
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
auto guard = getGuard();
return GetNonNullNames(_module<ControlThreadDataBuffer>().getActivatedNJointControllers());
}
void ControllerManagement::checkNJointControllerClassName(const std::string& className) const
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
if (!NJointControllerRegistry::has(className))
{
std::stringstream ss;
@@ -89,6 +92,7 @@ namespace armarx
std::vector<NJointControllerPtr> ControllerManagement::getNJointControllersNotNull(const std::vector<std::string>& names) const
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
auto guard = getGuard();
throwIfDevicesNotReady(__FUNCTION__);
std::vector<NJointControllerPtr> ctrl;
@@ -102,6 +106,7 @@ namespace armarx
const NJointControllerPtr& ControllerManagement::getNJointControllerNotNull(const std::string& name) const
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
auto guard = getGuard();
throwIfDevicesNotReady(__FUNCTION__);
auto it = nJointControllers.find(name);
@@ -125,11 +130,13 @@ namespace armarx
void ControllerManagement::deleteNJointController(const NJointControllerPtr& ctrl)
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
deleteNJointControllers({ctrl});
}
StringNJointControllerPrxDictionary ControllerManagement::getAllNJointControllers(const Ice::Current&) const
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
std::map<std::string, NJointControllerPtr> nJointControllersCopy;
{
auto guard = getGuard();
@@ -150,6 +157,7 @@ namespace armarx
const NJointControllerConfigPtr& config,
const Ice::Current&)
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
//no lock required
return NJointControllerInterfacePrx::uncheckedCast(
createNJointController(className, instanceName, config, true, false)->getProxy(-1, true));
@@ -161,6 +169,7 @@ namespace armarx
const StringVariantBaseMap& variants,
const Ice::Current&)
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
//no lock required
checkNJointControllerClassName(className);
if (!NJointControllerRegistry::get(className)->hasRemoteConfiguration())
@@ -186,6 +195,7 @@ namespace armarx
bool deletable,
bool internal)
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
auto guard = getGuard();
throwIfDevicesNotReady(__FUNCTION__);
if (instanceName.empty())
@@ -229,26 +239,31 @@ namespace armarx
bool ControllerManagement::loadLibFromPath(const std::string& path, const Ice::Current&)
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
return getArmarXManager()->loadLibFromPath(path);
}
bool ControllerManagement::loadLibFromPackage(const std::string& package, const std::string& lib, const Ice::Current&)
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
return getArmarXManager()->loadLibFromPackage(package, lib);
}
Ice::StringSeq ControllerManagement::getNJointControllerClassNames(const Ice::Current&) const
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
return NJointControllerRegistry::getKeys();
}
Ice::StringSeq ControllerManagement::getNJointControllerNames(const Ice::Current&) const
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
auto guard = getGuard();
return getMapKeys(nJointControllers);
}
std::vector<std::string> ControllerManagement::getNJointControllerNames(const std::vector<NJointControllerPtr>& ctrls) const
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
std::vector<std::string> result;
result.reserve(ctrls.size());
for (const auto& ctrl : ctrls)
@@ -263,18 +278,22 @@ namespace armarx
void ControllerManagement::activateNJointController(const std::string& name, const Ice::Current&)
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
activateNJointControllers(getNJointControllersNotNull({name}));
}
void ControllerManagement::activateNJointControllers(const Ice::StringSeq& names, const Ice::Current&)
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
activateNJointControllers(getNJointControllersNotNull(names));
}
void ControllerManagement::activateNJointController(const NJointControllerPtr& ctrl)
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
activateNJointControllers({ctrl});
}
void ControllerManagement::activateNJointControllers(const std::vector<NJointControllerPtr>& ctrlsToActVec)
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
if (ctrlsToActVec.empty())
{
return;
@@ -358,18 +377,22 @@ namespace armarx
void ControllerManagement::deactivateNJointController(const std::string& name, const Ice::Current&)
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
deactivateNJointControllers(getNJointControllersNotNull({name}));
}
void ControllerManagement::deactivateNJointControllers(const Ice::StringSeq& names, const Ice::Current&)
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
deactivateNJointControllers(getNJointControllersNotNull(names));
}
void ControllerManagement::deactivateNJointController(const NJointControllerPtr& ctrl)
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
deactivateNJointControllers({ctrl});
}
void ControllerManagement::deactivateNJointControllers(const std::vector<NJointControllerPtr>& ctrlsDeacVec)
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
auto guard = getGuard();
throwIfDevicesNotReady(__FUNCTION__);
if (ctrlsDeacVec.empty())
@@ -392,14 +415,17 @@ namespace armarx
void ControllerManagement::deleteNJointController(const std::string& name, const Ice::Current&)
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
deleteNJointControllers(getNJointControllersNotNull({name}));
}
void ControllerManagement::deleteNJointControllers(const Ice::StringSeq& names, const Ice::Current&)
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
deleteNJointControllers(getNJointControllersNotNull(names));
}
void ControllerManagement::switchNJointControllerSetup(const Ice::StringSeq& newSetup, const Ice::Current&)
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
auto guard = getGuard();
throwIfDevicesNotReady(__FUNCTION__);
auto ctrlsToActVec = getNJointControllersNotNull(newSetup); //also checks if these controllers exist
@@ -407,6 +433,7 @@ namespace armarx
}
void ControllerManagement::deleteNJointControllers(const std::vector<NJointControllerPtr>& ctrlsToDelVec)
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
auto guard = getGuard();
throwIfDevicesNotReady(__FUNCTION__);
if (ctrlsToDelVec.empty())
@@ -445,18 +472,22 @@ namespace armarx
void ControllerManagement::deactivateAndDeleteNJointController(const std::string& name, const Ice::Current&)
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
deactivateAndDeleteNJointControllers(getNJointControllersNotNull({name}));
}
void ControllerManagement::deactivateAndDeleteNJointControllers(const Ice::StringSeq& names, const Ice::Current&)
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
deactivateAndDeleteNJointControllers(getNJointControllersNotNull(names));
}
void ControllerManagement::deactivateAndDeleteNJointController(const NJointControllerPtr& ctrl)
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
deactivateAndDeleteNJointControllers({ctrl});
}
void ControllerManagement::deactivateAndDeleteNJointControllers(const std::vector<NJointControllerPtr>& ctrlsToDelVec)
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
auto guard = getGuard();
throwIfDevicesNotReady(__FUNCTION__);
if (ctrlsToDelVec.empty())
@@ -487,6 +518,7 @@ namespace armarx
NJointControllerClassDescription ControllerManagement::getNJointControllerClassDescription(
const std::string& className, const Ice::Current&) const
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
while (getRobotUnitState() == RobotUnitState::InitializingComponent || getRobotUnitState() == RobotUnitState::InitializingDevices)
{
//this phase should only last short so busy waiting is ok
@@ -511,6 +543,7 @@ namespace armarx
NJointControllerClassDescriptionSeq ControllerManagement::getNJointControllerClassDescriptions(const Ice::Current&) const
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
std::size_t tries = 200;
while (getRobotUnitState() == RobotUnitState::InitializingComponent || getRobotUnitState() == RobotUnitState::InitializingDevices)
{
@@ -533,6 +566,7 @@ namespace armarx
NJointControllerInterfacePrx ControllerManagement::getNJointController(const std::string& name, const Ice::Current&) const
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
NJointControllerPtr ctrl;
{
auto guard = getGuard();
@@ -548,6 +582,7 @@ namespace armarx
NJointControllerStatus ControllerManagement::getNJointControllerStatus(const std::string& name, const Ice::Current&) const
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
auto guard = getGuard();
throwIfDevicesNotReady(__FUNCTION__);
return getNJointControllerNotNull(name)->getControllerStatus();
@@ -555,6 +590,7 @@ namespace armarx
NJointControllerStatusSeq ControllerManagement::getNJointControllerStatuses(const Ice::Current&) const
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
auto guard = getGuard();
if (!areDevicesReady())
{
@@ -571,6 +607,7 @@ namespace armarx
NJointControllerDescription ControllerManagement::getNJointControllerDescription(const std::string& name, const Ice::Current&) const
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
NJointControllerPtr nJointCtrl;
{
auto guard = getGuard();
@@ -582,6 +619,7 @@ namespace armarx
NJointControllerDescriptionSeq ControllerManagement::getNJointControllerDescriptions(const Ice::Current&) const
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
std::map<std::string, NJointControllerPtr> nJointControllersCopy;
{
auto guard = getGuard();
@@ -603,6 +641,7 @@ namespace armarx
NJointControllerDescriptionWithStatus ControllerManagement::getNJointControllerDescriptionWithStatus(
const std::string& name, const Ice::Current&) const
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
NJointControllerPtr nJointCtrl;
{
auto guard = getGuard();
@@ -614,6 +653,7 @@ namespace armarx
NJointControllerDescriptionWithStatusSeq ControllerManagement::getNJointControllerDescriptionsWithStatuses(const Ice::Current&) const
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
std::map<std::string, NJointControllerPtr> nJointControllersCopy;
{
auto guard = getGuard();
@@ -634,6 +674,7 @@ namespace armarx
void ControllerManagement::removeNJointControllers(std::map<std::string, NJointControllerPtr>& ctrls, bool blocking, RobotUnitListenerPrx l)
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
for (auto& n2NJointCtrl : ctrls)
{
NJointControllerPtr& nJointCtrl = n2NJointCtrl.second;
@@ -657,11 +698,13 @@ namespace armarx
void ControllerManagement::removeNJointControllersToBeDeleted(bool blocking, RobotUnitListenerPrx l)
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
removeNJointControllers(nJointControllersToBeDeleted, blocking, l);
}
void ControllerManagement::_preFinishRunning()
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
//NJoint queued for deletion (some could still be in the queue)
ARMARX_DEBUG << "remove NJointControllers queued for deletion";
removeNJointControllersToBeDeleted();
@@ -674,16 +717,19 @@ namespace armarx
void ControllerManagement::_postFinishRunning()
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
nJointControllers.clear();
}
void ControllerManagement::_preOnInitRobotUnit()
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
controllerCreateRobot = _module<RobotData>().cloneRobot();
}
void ControllerManagement::updateNJointControllerRequestedState(const std::set<NJointControllerPtr>& request)
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
ARMARX_DEBUG << "set requested state for NJoint controllers";
for (const auto& name2NJoint : nJointControllers)
{
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.cpp
index 93d5d8280..3b43f53e2 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.cpp
@@ -33,7 +33,7 @@ namespace armarx
ControlDeviceDescription Devices::getControlDeviceDescription(const std::string& name, const Ice::Current&) const
{
- throwIfInControlThread(__FUNCTION__);
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
throwIfDevicesNotReady(__FUNCTION__);
std::lock_guard<MutexType> guard {controlDevicesMutex};
if (!controlDevices.has(name))
@@ -48,7 +48,7 @@ namespace armarx
ControlDeviceDescriptionSeq Devices::getControlDeviceDescriptions(const Ice::Current&) const
{
- throwIfInControlThread(__FUNCTION__);
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
if (!areDevicesReady())
{
return {};
@@ -66,21 +66,21 @@ namespace armarx
std::size_t Devices::getNumberOfControlDevices() const
{
- throwIfInControlThread(__FUNCTION__);
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
throwIfDevicesNotReady(__FUNCTION__);
return controlDevices.size();
}
std::size_t Devices::getNumberOfSensorDevices() const
{
- throwIfInControlThread(__FUNCTION__);
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
throwIfDevicesNotReady(__FUNCTION__);
return sensorDevices.size();
}
std::size_t Devices::getSensorDeviceIndex(const std::string& deviceName) const
{
- throwIfInControlThread(__FUNCTION__);
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
throwIfDevicesNotReady(__FUNCTION__);
std::lock_guard<MutexType> guard {sensorDevicesMutex};
ARMARX_CHECK_EXPRESSION(sensorDevices.has(deviceName));
@@ -88,7 +88,7 @@ namespace armarx
}
std::size_t Devices::getControlDeviceIndex(const std::string& deviceName) const
{
- throwIfInControlThread(__FUNCTION__);
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
throwIfDevicesNotReady(__FUNCTION__);
std::lock_guard<MutexType> guard {controlDevicesMutex};
ARMARX_CHECK_EXPRESSION(controlDevices.has(deviceName));
@@ -97,7 +97,7 @@ namespace armarx
ConstSensorDevicePtr Devices::getSensorDevice(const std::string& sensorDeviceName) const
{
- throwIfInControlThread(__FUNCTION__);
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
throwIfDevicesNotReady(__FUNCTION__);
std::lock_guard<MutexType> guard {sensorDevicesMutex};
return sensorDevices.at(sensorDeviceName, SensorDevice::NullPtr);
@@ -105,7 +105,7 @@ namespace armarx
ConstControlDevicePtr Devices::getControlDevice(const std::string& deviceName) const
{
- throwIfInControlThread(__FUNCTION__);
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
throwIfDevicesNotReady(__FUNCTION__);
std::lock_guard<MutexType> guard {controlDevicesMutex};
return controlDevices.at(deviceName, ControlDevice::NullPtr);
@@ -113,7 +113,7 @@ namespace armarx
Ice::StringSeq Devices::getControlDeviceNames(const Ice::Current&) const
{
- throwIfInControlThread(__FUNCTION__);
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
throwIfDevicesNotReady(__FUNCTION__);
std::lock_guard<MutexType> guard {controlDevicesMutex};
return controlDevices.keys();
@@ -121,7 +121,7 @@ namespace armarx
ControlDeviceDescription Devices::getControlDeviceDescription(std::size_t idx) const
{
- throwIfInControlThread(__FUNCTION__);
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
throwIfDevicesNotReady(__FUNCTION__);
std::lock_guard<MutexType> guard {controlDevicesMutex};
const ControlDevicePtr& controlDevice = controlDevices.at(idx);
@@ -139,6 +139,7 @@ namespace armarx
ControlDeviceStatus Devices::getControlDeviceStatus(std::size_t idx) const
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
throwIfDevicesNotReady(__FUNCTION__);
std::lock_guard<MutexType> guard {controlDevicesMutex};
const ControlDevicePtr& controlDevice = controlDevices.at(idx);
@@ -160,7 +161,7 @@ namespace armarx
ControlDeviceStatus Devices::getControlDeviceStatus(const std::string& name, const Ice::Current&) const
{
- throwIfInControlThread(__FUNCTION__);
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
throwIfDevicesNotReady(__FUNCTION__);
std::lock_guard<MutexType> guard {controlDevicesMutex};
if (!controlDevices.has(name))
@@ -175,7 +176,7 @@ namespace armarx
ControlDeviceStatusSeq Devices::getControlDeviceStatuses(const Ice::Current&) const
{
- throwIfInControlThread(__FUNCTION__);
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
if (!areDevicesReady())
{
return {};
@@ -193,7 +194,7 @@ namespace armarx
Ice::StringSeq Devices::getSensorDeviceNames(const Ice::Current&) const
{
- throwIfInControlThread(__FUNCTION__);
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
throwIfDevicesNotReady(__FUNCTION__);
std::lock_guard<MutexType> guard {sensorDevicesMutex};
return sensorDevices.keys();
@@ -201,7 +202,7 @@ namespace armarx
SensorDeviceDescription Devices::getSensorDeviceDescription(std::size_t idx) const
{
- throwIfInControlThread(__FUNCTION__);
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
throwIfDevicesNotReady(__FUNCTION__);
std::lock_guard<MutexType> guard {sensorDevicesMutex};
const SensorDevicePtr& sensorDevice = sensorDevices.at(idx);
@@ -215,6 +216,7 @@ namespace armarx
SensorDeviceStatus Devices::getSensorDeviceStatus(std::size_t idx) const
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
throwIfDevicesNotReady(__FUNCTION__);
std::lock_guard<MutexType> guard {sensorDevicesMutex};
const SensorDevicePtr& sensorDevice = sensorDevices.at(idx);
@@ -228,7 +230,7 @@ namespace armarx
SensorDeviceDescription Devices::getSensorDeviceDescription(const std::string& name, const Ice::Current&) const
{
- throwIfInControlThread(__FUNCTION__);
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
throwIfDevicesNotReady(__FUNCTION__);
std::lock_guard<MutexType> guard {sensorDevicesMutex};
if (!sensorDevices.has(name))
@@ -243,7 +245,7 @@ namespace armarx
SensorDeviceDescriptionSeq Devices::getSensorDeviceDescriptions(const Ice::Current&) const
{
- throwIfInControlThread(__FUNCTION__);
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
std::lock_guard<MutexType> guard {sensorDevicesMutex};
if (!areDevicesReady())
{
@@ -260,7 +262,7 @@ namespace armarx
SensorDeviceStatus Devices::getSensorDeviceStatus(const std::string& name, const Ice::Current&) const
{
- throwIfInControlThread(__FUNCTION__);
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
throwIfDevicesNotReady(__FUNCTION__);
std::lock_guard<MutexType> guard {sensorDevicesMutex};
if (!sensorDevices.has(name))
@@ -275,7 +277,7 @@ namespace armarx
SensorDeviceStatusSeq Devices::getSensorDeviceStatuses(const Ice::Current&) const
{
- throwIfInControlThread(__FUNCTION__);
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
if (!areDevicesReady())
{
return {};
@@ -293,6 +295,7 @@ namespace armarx
void Devices::addControlDevice(const ControlDevicePtr& cd)
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
ARMARX_DEBUG << "ControlDevice " << &cd;
throwIfStateIsNot(RobotUnitState::InitializingDevices, __FUNCTION__);
{
@@ -333,6 +336,7 @@ namespace armarx
void Devices::addSensorDevice(const SensorDevicePtr& sd)
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
ARMARX_DEBUG << "SensorDevice " << &sd;
throwIfStateIsNot(RobotUnitState::InitializingDevices, __FUNCTION__);
{
@@ -384,13 +388,13 @@ namespace armarx
RTThreadTimingsSensorDevicePtr Devices::createRTThreadTimingSensorDevice() const
{
- throwIfInControlThread(__FUNCTION__);
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
return std::make_shared<RTThreadTimingsSensorDeviceImpl<>>(rtThreadTimingsSensorDeviceName);
}
void Devices::_postFinishRunning()
{
- throwIfInControlThread(__FUNCTION__);
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
std::lock_guard<MutexType> guardS {sensorDevicesMutex};
std::lock_guard<MutexType> guardC {controlDevicesMutex};
controlDevicesConstPtr.clear();
@@ -401,8 +405,8 @@ namespace armarx
std::vector<JointController*> Devices::getStopMovementJointControllers() const
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
throwIfDevicesNotReady(__FUNCTION__);
- throwIfInControlThread(__FUNCTION__);
std::lock_guard<MutexType> guard {controlDevicesMutex};
std::vector<JointController*> controllers;
controllers.reserve(controlDevices.values().size());
@@ -419,8 +423,8 @@ namespace armarx
std::vector<JointController*> Devices::getEmergencyStopJointControllers() const
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
throwIfDevicesNotReady(__FUNCTION__);
- throwIfInControlThread(__FUNCTION__);
std::lock_guard<MutexType> guard {controlDevicesMutex};
std::vector<JointController*> controllers;
controllers.reserve(controlDevices.values().size());
@@ -437,7 +441,7 @@ namespace armarx
void Devices::_preFinishDeviceInitialization()
{
- throwIfInControlThread(__FUNCTION__);
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
std::lock_guard<MutexType> guardS {sensorDevicesMutex};
std::lock_guard<MutexType> guardC {controlDevicesMutex};
if (!sensorDevices.has(rtThreadTimingsSensorDeviceName))
@@ -448,7 +452,7 @@ namespace armarx
void Devices::_postFinishDeviceInitialization()
{
- throwIfInControlThread(__FUNCTION__);
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
std::lock_guard<MutexType> guardS {sensorDevicesMutex};
std::lock_guard<MutexType> guardC {controlDevicesMutex};
ARMARX_DEBUG << "checking " << controlDevices.size() << " ControlDevices:";
@@ -668,7 +672,7 @@ namespace armarx
void Devices::_preOnInitRobotUnit()
{
- throwIfInControlThread(__FUNCTION__);
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
//ControlDeviceHardwareControlModeGroups
const std::string controlDeviceHardwareControlModeGroupsStr = getProperty<std::string>("ControlDeviceHardwareControlModeGroups").getValue();
if (!controlDeviceHardwareControlModeGroupsStr.empty())
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.cpp
index e3fd16636..3502aefce 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.cpp
@@ -36,6 +36,7 @@ namespace armarx
{
void Logging::addMarkerToRtLog(const RemoteReferenceCounterBasePtr& token, const std::string& marker, const Ice::Current&)
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
std::lock_guard<std::mutex> guard {rtLoggingMutex};
if (!rtLoggingEntries.count(token->getId()))
{
@@ -46,6 +47,7 @@ namespace armarx
RemoteReferenceCounterBasePtr Logging::startRtLogging(const std::string& formatString, const Ice::StringSeq& loggingNames, const Ice::Current&)
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
StringStringDictionary alias;
for (const auto& name : loggingNames)
{
@@ -56,6 +58,7 @@ namespace armarx
void Logging::stopRtLogging(const RemoteReferenceCounterBasePtr& token, const Ice::Current&)
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
std::lock_guard<std::mutex> guard {rtLoggingMutex};
if (!rtLoggingEntries.count(token->getId()))
{
@@ -67,6 +70,7 @@ namespace armarx
Ice::StringSeq Logging::getLoggingNames(const Ice::Current&) const
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
Ice::StringSeq result;
for (const auto& strs : sensorDeviceValueLoggingNames)
{
@@ -84,6 +88,7 @@ namespace armarx
RemoteReferenceCounterBasePtr Logging::startRtLoggingWithAliasNames(const std::string& formatString, const StringStringDictionary& aliasNames, const Ice::Current&)
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
FileSystemPathBuilder pb {formatString};
std::lock_guard<std::mutex> guard {rtLoggingMutex};
if (rtLoggingEntries.count(pb.getPath()))
@@ -176,6 +181,7 @@ namespace armarx
void Logging::writeRecentIterationsToFile(const std::string& formatString, const Ice::Current&) const
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
std::lock_guard<std::mutex> guard {rtLoggingMutex};
FileSystemPathBuilder pb {formatString};
pb.createParentDirectories();
@@ -265,6 +271,7 @@ namespace armarx
void Logging::_preFinishRunning()
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
defaultLogHandle = nullptr;
if (rtLoggingTask)
{
@@ -281,6 +288,7 @@ namespace armarx
void Logging::_preFinishControlThreadInitialization()
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
controlThreadId = LogSender::getThreadId();
ControlThreadOutputBuffer::RtLoggingInstance = &(_module<ControlThreadDataBuffer>().getControlThreadOutputBuffer());
@@ -290,6 +298,7 @@ namespace armarx
void Logging::doLogging()
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
std::lock_guard<std::mutex> guard {rtLoggingMutex};
const auto now = IceUtil::Time::now();
// entries are removed last
@@ -411,6 +420,7 @@ namespace armarx
void Logging::_postOnInitRobotUnit()
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
rtLoggingTimestep = getProperty<std::size_t>("RTLoggingPeriodMs");
ARMARX_CHECK_LESS_W_HINT(0, rtLoggingTimestep, "The property RTLoggingPeriodMs must not be 0");
@@ -429,6 +439,7 @@ namespace armarx
void Logging::_postFinishDeviceInitialization()
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
//init buffer
{
std::size_t ctrlThreadPeriodUs = static_cast<std::size_t>(getControlThreadTargetPeriod().toMicroSeconds());
@@ -498,6 +509,5 @@ namespace armarx
rtLoggingTask = new RTLoggingTaskT([&] {doLogging();}, rtLoggingTimestep, false, getName() + "_RTLoggingTask");
rtLoggingTask->setDelayWarningTolerance(rtLoggingTimestep * 10);
}
-
}
}
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleManagement.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleManagement.cpp
index b532f3f47..ce3322524 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleManagement.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleManagement.cpp
@@ -30,6 +30,7 @@ namespace armarx
{
void Management::_preOnInitRobotUnit()
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
additionalObjectSchedulerCount = getProperty<std::uint64_t>("AdditionalObjectSchedulerCount").getValue();
getArmarXManager()->increaseSchedulers(static_cast<int>(additionalObjectSchedulerCount));
}
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleManagement.h b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleManagement.h
index 365e7d183..cdc5db9aa 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleManagement.h
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleManagement.h
@@ -74,6 +74,7 @@ namespace armarx
*/
bool isRunning(const Ice::Current& = GlobalIceCurrent) const override
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
return getRobotUnitState() == RobotUnitState::Running;
}
// //////////////////////////////////////////////////////////////////////////////////////// //
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.cpp
index f0a4f5091..3049de191 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.cpp
@@ -117,11 +117,13 @@ namespace armarx
{
const std::map<std::string, NJointControllerPtr>& Publisher::getNJointControllers()
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
return ControllerManagementAttorneyForPublisher::GetNJointControllers(this);
}
TimedVariantPtr Publisher::publishNJointClassNames(IceUtil::Time timestamp, RobotUnitListenerPrx& listenerPrx)
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
const auto beg = TimeUtil::GetTime(true);
const auto classNames = NJointControllerRegistry::getKeys();
@@ -149,6 +151,7 @@ namespace armarx
DebugObserverInterfacePrx& debugObserverBatchPrx
)
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
const auto beg = TimeUtil::GetTime(true);
for (const auto& pair : getNJointControllers())
@@ -181,6 +184,7 @@ namespace armarx
const SensorAndControl& controlThreadOutputBuffer,
const JointAndNJointControllers& activatedControllers)
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
const auto beg = TimeUtil::GetTime(true);
ARMARX_DEBUG << deactivateSpam(spamdelay) << "updating units with new sensor values";
@@ -209,6 +213,7 @@ namespace armarx
const JointAndNJointControllers& activatedControllers,
const std::vector<JointController*>& requestedJointControllers)
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
const auto beg = TimeUtil::GetTime(true);
@@ -263,6 +268,7 @@ namespace armarx
bool publishToObserver,
const SensorAndControl& controlThreadOutputBuffer)
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
const auto beg = TimeUtil::GetTime(true);
StringVariantBaseMap sensorDevMap;
@@ -313,39 +319,46 @@ namespace armarx
std::string Publisher::getRobotUnitListenerTopicName(const Ice::Current&) const
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
return robotUnitListenerTopicName;
}
std::string Publisher::getDebugDrawerTopicName(const Ice::Current&) const
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
return debugDrawerUpdatesTopicName;
}
std::string Publisher::getDebugObserverTopicName(const Ice::Current&) const
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
return debugObserverTopicName;
}
RobotUnitListenerPrx Publisher::getRobotUnitListenerProxy(const Ice::Current&) const
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
ARMARX_CHECK_EXPRESSION(robotUnitListenerPrx);
return robotUnitListenerPrx;
}
DebugObserverInterfacePrx Publisher::getDebugObserverProxy(const Ice::Current&) const
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
ARMARX_CHECK_EXPRESSION(debugObserverPrx);
return debugObserverPrx;
}
DebugDrawerInterfacePrx Publisher::getDebugDrawerProxy(const Ice::Current&) const
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
ARMARX_CHECK_EXPRESSION(debugDrawerPrx);
return debugDrawerPrx;
}
void Publisher::_preOnConnectRobotUnit()
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
ARMARX_DEBUG << "retrieving RobotUnitListenerPrx for topic " << getRobotUnitListenerTopicName();
robotUnitListenerPrx = getTopic<RobotUnitListenerPrx>(getRobotUnitListenerTopicName());
@@ -358,6 +371,7 @@ namespace armarx
void Publisher::_preOnInitRobotUnit()
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
robotUnitListenerTopicName = getProperty<std::string>("RobotUnitListenerTopicName").getValue();
debugDrawerUpdatesTopicName = getProperty<std::string>("DebugDrawerUpdatesTopicName").getValue();
debugObserverTopicName = getProperty<std::string>("DebugObserverTopicName").getValue();
@@ -376,12 +390,14 @@ namespace armarx
void Publisher::_icePropertiesInitialized()
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
robotUnitObserver = Component::create<RobotUnitObserver, RobotUnitObserverPtr>(getIceProperties(), "", getConfigDomain());
addPropertyUser(PropertyUserPtr::dynamicCast(robotUnitObserver));
}
void Publisher::_icePropertiesUpdated(const std::set<std::string>& changedProperties)
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
if (changedProperties.count("ObserverPublishSensorValues"))
{
observerPublishSensorValues = getProperty<bool>("ObserverPublishSensorValues").getValue();
@@ -398,6 +414,7 @@ namespace armarx
void Publisher::_preFinishRunning()
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
if (publisherTask)
{
ARMARX_DEBUG << "shutting down publisher task";
@@ -431,6 +448,7 @@ namespace armarx
void Publisher::publish(StringVariantBaseMap additionalMap, StringVariantBaseMap timingMap)
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
auto begPublish = TimeUtil::GetTime(true);
if (getRobotUnitState() != RobotUnitState::Running)
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.cpp
index ab976c3e4..78a455b5e 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.cpp
@@ -34,30 +34,35 @@ namespace armarx
{
const std::string& RobotData::getRobotPlatformName() const
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
ARMARX_CHECK_EXPRESSION_W_HINT(robot, __FUNCTION__ << " should only be called after _initVirtualRobot was called");
return robotPlatformName;
}
const std::string& RobotData::getRobotNodetSeName() const
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
ARMARX_CHECK_EXPRESSION_W_HINT(robot, __FUNCTION__ << " should only be called after _initVirtualRobot was called");
return robotNodeSetName;
}
const std::string& RobotData::getRobotProjectName() const
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
ARMARX_CHECK_EXPRESSION_W_HINT(robot, __FUNCTION__ << " should only be called after _initVirtualRobot was called");
return robotProjectName;
}
const std::string& RobotData::getRobotFileName() const
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
ARMARX_CHECK_EXPRESSION_W_HINT(robot, __FUNCTION__ << " should only be called after _initVirtualRobot was called");
return robotFileName;
}
std::string RobotData::getRobotName() const
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
std::lock_guard<std::mutex> guard {robotMutex};
ARMARX_CHECK_EXPRESSION_W_HINT(robot, __FUNCTION__ << " should only be called after _initVirtualRobot was called");
return robot->getName();
@@ -65,6 +70,7 @@ namespace armarx
VirtualRobot::RobotPtr RobotData::cloneRobot(bool updateCollisionModel) const
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
std::lock_guard<std::mutex> guard {robotMutex};
ARMARX_CHECK_EXPRESSION_W_HINT(robot, __FUNCTION__ << " should only be called after _initVirtualRobot was called");
ARMARX_CHECK_EXPRESSION(robot);
@@ -74,8 +80,9 @@ namespace armarx
return clone;
}
- void armarx::RobotUnitModule::RobotData::_initVirtualRobot()
+ void RobotData::_initVirtualRobot()
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
std::lock_guard<std::mutex> guard {robotMutex};
ARMARX_CHECK_IS_NULL(robot);
robotNodeSetName = getProperty<std::string>("RobotNodeSetName").getValue();
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleSelfCollisionChecker.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleSelfCollisionChecker.cpp
index 46a45683e..714aed26f 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleSelfCollisionChecker.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleSelfCollisionChecker.cpp
@@ -34,6 +34,7 @@ namespace armarx
{
void SelfCollisionChecker::_preOnInitRobotUnit()
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
{
const std::string colModelsString = getProperty<std::string>("SelfCollisionpairsToCheck").getValue();
std::vector<std::string> groups;
@@ -116,6 +117,7 @@ namespace armarx
void SelfCollisionChecker::setSelfCollisionAvoidanceDistance(Ice::Float distance, const Ice::Current&)
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
std::lock_guard<std::mutex> guard {selfCollisionDataMutex};
if (distance < 0)
{
@@ -169,6 +171,7 @@ namespace armarx
void SelfCollisionChecker::setSelfCollisionAvoidanceFrequency(Ice::Float freq, const Ice::Current&)
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
if (freq < 0)
{
throw InvalidArgumentException
@@ -181,21 +184,25 @@ namespace armarx
bool SelfCollisionChecker::isSelfCollisionCheckEnabled(const Ice::Current&) const
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
return checkFrequency != 0;
}
float SelfCollisionChecker::getSelfCollisionAvoidanceFrequency(const Ice::Current&) const
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
return checkFrequency;
}
float SelfCollisionChecker::getSelfCollisionAvoidanceDistance(const Ice::Current&) const
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
return minSelfDistance;
}
void SelfCollisionChecker::componentPropertiesUpdated(const std::set<std::string>& changed)
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
if (changed.count("SelfCollisionCheckFrequency"))
{
setSelfCollisionAvoidanceFrequency(getProperty<float>("SelfCollisionCheckFrequency").getValue());
@@ -213,6 +220,7 @@ namespace armarx
void SelfCollisionChecker::selfCollisionAvoidanceTask()
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
while (true)
{
const auto startT = std::chrono::high_resolution_clock::now();
@@ -271,6 +279,7 @@ namespace armarx
void SelfCollisionChecker::_preFinishRunning()
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
ARMARX_INFO << "Stopping Self Collision Avoidance.";
if (selfCollisionAvoidanceThread.joinable())
{
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp
index cb344901d..992fc7572 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp
@@ -97,6 +97,7 @@ namespace armarx
{
Ice::ObjectProxySeq Units::getUnits(const Ice::Current&) const
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
std::vector<ManagedIceObjectPtr> unitsCopy;
{
auto guard = getGuard();
@@ -114,6 +115,7 @@ namespace armarx
Ice::ObjectPrx Units::getUnit(const std::string& staticIceId, const Ice::Current&) const
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
//no lock required
ManagedIceObjectPtr unit = getUnit(staticIceId);
if (unit)
@@ -125,11 +127,13 @@ namespace armarx
const EmergencyStopMasterInterfacePtr& Units::getEmergencyStopMaster() const
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
return emergencyStopMaster;
}
void Units::initializeDefaultUnits()
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
auto beg = TimeUtil::GetTime(true);
{
auto guard = getGuard();
@@ -151,9 +155,9 @@ namespace armarx
ARMARX_INFO << "initializing default units...done! " << (TimeUtil::GetTime(true) - beg).toMicroSeconds() << " us";
}
-
void Units::initializeKinematicUnit()
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
using IfaceT = KinematicUnitInterface;
auto guard = getGuard();
@@ -178,6 +182,7 @@ namespace armarx
const std::string& torqueControlMode
)
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
/// TODO move init code to Kinematic sub unit
using UnitT = KinematicSubUnit;
@@ -258,6 +263,7 @@ namespace armarx
void Units::initializePlatformUnit()
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
using UnitT = PlatformSubUnit;
using IfaceT = PlatformUnitInterface;
@@ -315,6 +321,7 @@ namespace armarx
void Units::initializeForceTorqueUnit()
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
using UnitT = ForceTorqueSubUnit;
using IfaceT = ForceTorqueUnitInterface;
@@ -371,6 +378,7 @@ namespace armarx
void Units::initializeInertialMeasurementUnit()
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
using UnitT = InertialMeasurementSubUnit;
using IfaceT = InertialMeasurementUnitInterface;
@@ -411,6 +419,7 @@ namespace armarx
void Units::initializeTrajectoryControllerUnit()
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
auto guard = getGuard();
throwIfStateIsNot(RobotUnitState::InitializingUnits, __FUNCTION__);
using UnitT = TrajectoryControllerSubUnit;
@@ -431,6 +440,7 @@ namespace armarx
void Units::initializeTcpControllerUnit()
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
auto guard = getGuard();
throwIfStateIsNot(RobotUnitState::InitializingUnits, __FUNCTION__);
@@ -452,6 +462,7 @@ namespace armarx
void Units::addUnit(ManagedIceObjectPtr unit)
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
ARMARX_CHECK_NOT_NULL(unit);
auto guard = getGuard();
throwIfStateIsNot(RobotUnitState::InitializingUnits, __FUNCTION__);
@@ -467,6 +478,7 @@ namespace armarx
void Units::_icePropertiesInitialized()
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
Ice::PropertiesPtr properties = getIceProperties()->clone();
const std::string& configDomain = "ArmarX";
// create TCPControlSubUnit
@@ -490,6 +502,7 @@ namespace armarx
void Units::_preFinishRunning()
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
//remove all units
subUnits.clear();
for (ManagedIceObjectPtr& unit : units)
@@ -501,6 +514,7 @@ namespace armarx
void Units::_preOnInitRobotUnit()
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
unitCreateRobot = _module<RobotData>().cloneRobot();
ARMARX_DEBUG << "add emergency stop master";
{
@@ -527,6 +541,7 @@ namespace armarx
void Units::_postOnExitRobotUnit()
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
ARMARX_DEBUG << "remove EmergencyStopMaster";
try
{
@@ -539,6 +554,7 @@ namespace armarx
const ManagedIceObjectPtr& Units::getUnit(const std::string& staticIceId) const
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
auto guard = getGuard();
for (const ManagedIceObjectPtr& unit : units)
{
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.ipp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.ipp
index 557577805..c9e6e111e 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.ipp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.ipp
@@ -35,63 +35,77 @@ namespace armarx
inline KinematicUnitInterfacePrx Units::getKinematicUnit(const Ice::Current&) const
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
return getUnitPrx<KinematicUnitInterface>();
}
inline ForceTorqueUnitInterfacePrx Units::getForceTorqueUnit(const Ice::Current&) const
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
return getUnitPrx<ForceTorqueUnitInterface>();
}
inline InertialMeasurementUnitInterfacePrx Units::getInertialMeasurementUnit(const Ice::Current&) const
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
return getUnitPrx<InertialMeasurementUnitInterface>();
}
inline PlatformUnitInterfacePrx Units::getPlatformUnit(const Ice::Current&) const
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
return getUnitPrx<PlatformUnitInterface>();
}
inline TCPControlUnitInterfacePrx Units::getTCPControlUnit(const Ice::Current&) const
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
return getUnitPrx<TCPControlUnitInterface>();
}
inline TrajectoryPlayerInterfacePrx Units::getTrajectoryPlayer(const Ice::Current&) const
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
return getUnitPrx<TrajectoryPlayerInterface>();
}
template<class T>
inline typename T::PointerType Units::getUnit() const
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
return T::PointerType::dynamicCast(getUnit(T::ice_staticId()));
}
template<class T>
inline typename T::ProxyType Units::getUnitPrx() const
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
return T::ProxyType::uncheckedCast(getUnit(T::ice_staticId(), GlobalIceCurrent));
}
inline KinematicUnitInterfacePtr Units::getKinematicUnit() const
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
return getUnit<KinematicUnitInterface>();
}
inline ForceTorqueUnitInterfacePtr Units::getForceTorqueUnit() const
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
return getUnit<ForceTorqueUnitInterface>();
}
inline InertialMeasurementUnitInterfacePtr Units::getInertialMeasurementUnit() const
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
return getUnit<InertialMeasurementUnitInterface>();
}
inline PlatformUnitInterfacePtr Units::getPlatformUnit() const
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
return getUnit<PlatformUnitInterface>();
}
inline TCPControlUnitInterfacePtr Units::getTCPControlUnit() const
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
return getUnit<TCPControlUnitInterface>();
}
inline TrajectoryPlayerInterfacePtr Units::getTrajectoryPlayer() const
{
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
return getUnit<TrajectoryPlayerInterface>();
}
}
--
GitLab