diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.cpp
index fa21a0fcecea44d404c180fc373fc9f1708d9605..61622e70524f2cc8285aa131ace437f80ce6aab9 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.cpp
@@ -35,6 +35,7 @@ namespace armarx
{
throwIfInControlThread(__FUNCTION__);
throwIfDevicesNotReady(__FUNCTION__);
+ std::lock_guard<MutexType> guard {controlDevicesMutex};
if (!controlDevices.has(name))
{
std::stringstream ss;
@@ -52,12 +53,14 @@ namespace armarx
{
return {};
}
+ std::lock_guard<MutexType> guard {controlDevicesMutex};
ControlDeviceDescriptionSeq r;
r.reserve(getNumberOfControlDevices());
for (auto idx : getIndices(controlDevices.values()))
{
r.emplace_back(getControlDeviceDescription(idx));
}
+ throwIfDevicesNotReady(__FUNCTION__);
return r;
}
@@ -65,6 +68,7 @@ namespace armarx
{
throwIfInControlThread(__FUNCTION__);
throwIfDevicesNotReady(__FUNCTION__);
+ std::lock_guard<MutexType> guard {controlDevicesMutex};
return controlDevices.size();
}
@@ -72,6 +76,7 @@ namespace armarx
{
throwIfInControlThread(__FUNCTION__);
throwIfDevicesNotReady(__FUNCTION__);
+ std::lock_guard<MutexType> guard {sensorDevicesMutex};
return sensorDevices.size();
}
@@ -79,6 +84,7 @@ namespace armarx
{
throwIfInControlThread(__FUNCTION__);
throwIfDevicesNotReady(__FUNCTION__);
+ std::lock_guard<MutexType> guard {sensorDevicesMutex};
ARMARX_CHECK_EXPRESSION(sensorDevices.has(deviceName));
return sensorDevices.index(deviceName);
}
@@ -86,6 +92,7 @@ namespace armarx
{
throwIfInControlThread(__FUNCTION__);
throwIfDevicesNotReady(__FUNCTION__);
+ std::lock_guard<MutexType> guard {controlDevicesMutex};
ARMARX_CHECK_EXPRESSION(controlDevices.has(deviceName));
return controlDevices.index(deviceName);
}
@@ -94,6 +101,7 @@ namespace armarx
{
throwIfInControlThread(__FUNCTION__);
throwIfDevicesNotReady(__FUNCTION__);
+ std::lock_guard<MutexType> guard {sensorDevicesMutex};
return sensorDevices.at(sensorDeviceName, SensorDevice::NullPtr);
}
@@ -101,6 +109,7 @@ namespace armarx
{
throwIfInControlThread(__FUNCTION__);
throwIfDevicesNotReady(__FUNCTION__);
+ std::lock_guard<MutexType> guard {controlDevicesMutex};
return controlDevices.at(deviceName, ControlDevice::NullPtr);
}
@@ -108,6 +117,7 @@ namespace armarx
{
throwIfInControlThread(__FUNCTION__);
throwIfDevicesNotReady(__FUNCTION__);
+ std::lock_guard<MutexType> guard {controlDevicesMutex};
return controlDevices.keys();
}
@@ -115,6 +125,7 @@ namespace armarx
{
throwIfInControlThread(__FUNCTION__);
throwIfDevicesNotReady(__FUNCTION__);
+ std::lock_guard<MutexType> guard {controlDevicesMutex};
const ControlDevicePtr& controlDevice = controlDevices.at(idx);
ControlDeviceDescription data;
data.deviceName = controlDevice->getDeviceName();
@@ -124,13 +135,14 @@ namespace armarx
data.contolModeToTargetType[jointCtrl->getControlMode()].targetType = jointCtrl->getControlTarget()->getControlTargetType();
data.contolModeToTargetType[jointCtrl->getControlMode()].hardwareControlMode = jointCtrl->getHardwareControlMode();
}
+ throwIfDevicesNotReady(__FUNCTION__);
return data;
}
ControlDeviceStatus Devices::getControlDeviceStatus(std::size_t idx) const
{
- auto guard = getGuard();
throwIfDevicesNotReady(__FUNCTION__);
+ std::lock_guard<MutexType> guard {controlDevicesMutex};
const ControlDevicePtr& controlDevice = controlDevices.at(idx);
ControlDeviceStatus status;
const auto activeJointCtrl = _module<ControlThreadDataBuffer>().getActivatedJointControllers().at(idx);
@@ -144,6 +156,7 @@ namespace armarx
status.controlTargetValues[targ->getControlMode()] = targ->toVariants(_module<ControlThreadDataBuffer>().getSensorAndControlBuffer().sensorValuesTimestamp);
}
status.timestampUSec = MicroNow().count();
+ throwIfDevicesNotReady(__FUNCTION__);
return status;
}
@@ -151,6 +164,7 @@ namespace armarx
{
throwIfInControlThread(__FUNCTION__);
throwIfDevicesNotReady(__FUNCTION__);
+ std::lock_guard<MutexType> guard {controlDevicesMutex};
if (!controlDevices.has(name))
{
std::stringstream ss;
@@ -168,12 +182,14 @@ namespace armarx
{
return {};
}
+ std::lock_guard<MutexType> guard {controlDevicesMutex};
ControlDeviceStatusSeq r;
r.reserve(getNumberOfControlDevices());
for (auto idx : getIndices(controlDevices.values()))
{
r.emplace_back(getControlDeviceStatus(idx));
}
+ throwIfDevicesNotReady(__FUNCTION__);
return r;
}
@@ -181,6 +197,7 @@ namespace armarx
{
throwIfInControlThread(__FUNCTION__);
throwIfDevicesNotReady(__FUNCTION__);
+ std::lock_guard<MutexType> guard {sensorDevicesMutex};
return sensorDevices.keys();
}
@@ -188,23 +205,26 @@ namespace armarx
{
throwIfInControlThread(__FUNCTION__);
throwIfDevicesNotReady(__FUNCTION__);
+ std::lock_guard<MutexType> guard {sensorDevicesMutex};
const SensorDevicePtr& sensorDevice = sensorDevices.at(idx);
SensorDeviceDescription data;
data.deviceName = sensorDevice->getDeviceName();
data.tags.assign(sensorDevice->getTags().begin(), sensorDevice->getTags().end());
data.sensorValueType = sensorDevice->getSensorValueType();
+ throwIfDevicesNotReady(__FUNCTION__);
return data;
}
SensorDeviceStatus Devices::getSensorDeviceStatus(std::size_t idx) const
{
- auto guard = getGuard();
throwIfDevicesNotReady(__FUNCTION__);
+ std::lock_guard<MutexType> guard {sensorDevicesMutex};
const SensorDevicePtr& sensorDevice = sensorDevices.at(idx);
SensorDeviceStatus status;
status.deviceName = sensorDevice->getDeviceName();
status.sensorValue = _module<ControlThreadDataBuffer>().getSensorAndControlBuffer().sensors.at(idx)->toVariants(_module<ControlThreadDataBuffer>().getSensorAndControlBuffer().sensorValuesTimestamp);
status.timestampUSec = MicroNow().count();
+ throwIfDevicesNotReady(__FUNCTION__);
return status;
}
@@ -212,6 +232,7 @@ namespace armarx
{
throwIfInControlThread(__FUNCTION__);
throwIfDevicesNotReady(__FUNCTION__);
+ std::lock_guard<MutexType> guard {sensorDevicesMutex};
if (!sensorDevices.has(name))
{
std::stringstream ss;
@@ -225,6 +246,7 @@ namespace armarx
SensorDeviceDescriptionSeq Devices::getSensorDeviceDescriptions(const Ice::Current&) const
{
throwIfInControlThread(__FUNCTION__);
+ std::lock_guard<MutexType> guard {sensorDevicesMutex};
if (!areDevicesReady())
{
return {};
@@ -242,6 +264,7 @@ namespace armarx
{
throwIfInControlThread(__FUNCTION__);
throwIfDevicesNotReady(__FUNCTION__);
+ std::lock_guard<MutexType> guard {sensorDevicesMutex};
if (!sensorDevices.has(name))
{
std::stringstream ss;
@@ -259,72 +282,103 @@ namespace armarx
{
return {};
}
+ std::lock_guard<MutexType> guard {sensorDevicesMutex};
SensorDeviceStatusSeq r;
r.reserve(getNumberOfSensorDevices());
for (auto idx : getIndices(sensorDevices.values()))
{
r.emplace_back(getSensorDeviceStatus(idx));
}
+ throwIfDevicesNotReady(__FUNCTION__);
return r;
}
void Devices::addControlDevice(const ControlDevicePtr& cd)
{
- throwIfInControlThread(__FUNCTION__);
- //this guards prevents the RobotUnitState to change
- auto guard = getGuard();
- ARMARX_DEBUG << "Check RobotUnit State";
+ ARMARX_DEBUG << "ControlDevice " << &cd;
throwIfStateIsNot(RobotUnitState::InitializingDevices, __FUNCTION__);
- ARMARX_DEBUG << "Adding the ControlDevice " << cd->getDeviceName() << " " << &cd ;
- controlDevices.add(cd->getDeviceName(), cd);
+ {
+ std::lock_guard<MutexType> guard {controlDevicesMutex};
+ //check it
+ if (!cd)
+ {
+ std::stringstream ss;
+ ss << "armarx::RobotUnit::addControlDevice: ControlDevice is nullptr";
+ ARMARX_ERROR << ss.str();
+ throw InvalidArgumentException {ss.str()};
+ }
+ if (!cd->getJointEmergencyStopController())
+ {
+ std::stringstream ss;
+ ss << "armarx::RobotUnit::addControlDevice: ControlDevice " << cd->getDeviceName()
+ << " has null JointEmergencyStopController (this is not allowed)";
+ ARMARX_ERROR << ss.str();
+ throw InvalidArgumentException {ss.str()};
+ }
+ if (!cd->getJointStopMovementController())
+ {
+ std::stringstream ss;
+ ss << "armarx::RobotUnit::addControlDevice: ControlDevice " << cd->getDeviceName()
+ << " has null getJointStopMovementController (this is not allowed)";
+ ARMARX_ERROR << ss.str();
+ throw InvalidArgumentException {ss.str()};
+ }
+ //add it
+ ARMARX_DEBUG << "Adding the ControlDevice " << cd->getDeviceName() << " " << &cd ;
+ controlDevices.add(cd->getDeviceName(), cd);
+ ARMARX_INFO << "added ControlDevice " << cd->getDeviceName();
+ }
ARMARX_INFO << "added ControlDevice " << cd->getDeviceName();
+ throwIfStateIsNot(RobotUnitState::InitializingDevices, __FUNCTION__);
}
void Devices::addSensorDevice(const SensorDevicePtr& sd)
{
ARMARX_DEBUG << "SensorDevice " << &sd;
- throwIfInControlThread(__FUNCTION__);
- //this guards prevents the RobotUnitState to change
- auto guard = getGuard();
- ARMARX_DEBUG << "Check RobotUnit State";
throwIfStateIsNot(RobotUnitState::InitializingDevices, __FUNCTION__);
- if (!sd)
- {
- std::stringstream ss;
- ss << "armarx::RobotUnit::addSensorDevice: SensorDevice is nullptr";
- ARMARX_ERROR << ss.str();
- throw InvalidArgumentException {ss.str()};
- }
- if (!sd->getSensorValue())
- {
- std::stringstream ss;
- ss << "armarx::RobotUnit::addSensorDevice: SensorDevice " << sd->getDeviceName()
- << " has null SensorValue (this is not allowed)";
- ARMARX_ERROR << ss.str();
- throw InvalidArgumentException {ss.str()};
- }
- if (sd->getDeviceName() == rtThreadTimingsSensorDeviceName)
{
- ARMARX_DEBUG << "Device is the " << rtThreadTimingsSensorDeviceName;
- if (!std::dynamic_pointer_cast<RTThreadTimingsSensorDevice>(sd))
+ std::lock_guard<MutexType> guard {sensorDevicesMutex};
+ //check it
+ if (!sd)
{
- throw InvalidArgumentException
+ std::stringstream ss;
+ ss << "armarx::RobotUnit::addSensorDevice: SensorDevice is nullptr";
+ ARMARX_ERROR << ss.str();
+ throw InvalidArgumentException {ss.str()};
+ }
+ if (!sd->getSensorValue())
+ {
+ std::stringstream ss;
+ ss << "armarx::RobotUnit::addSensorDevice: SensorDevice " << sd->getDeviceName()
+ << " has null SensorValue (this is not allowed)";
+ ARMARX_ERROR << ss.str();
+ throw InvalidArgumentException {ss.str()};
+ }
+ //add it
+ if (sd->getDeviceName() == rtThreadTimingsSensorDeviceName)
+ {
+ ARMARX_DEBUG << "Device is the " << rtThreadTimingsSensorDeviceName;
+ if (!std::dynamic_pointer_cast<RTThreadTimingsSensorDevice>(sd))
{
- "You tried to add a SensorDevice with the name " + sd->getDeviceName() +
- " which does not derive from RTThreadTimingsSensorDevice. (Don't do this)"
- };
+ throw InvalidArgumentException
+ {
+ "You tried to add a SensorDevice with the name " + sd->getDeviceName() +
+ " which does not derive from RTThreadTimingsSensorDevice. (Don't do this)"
+ };
+ }
+ //this checks if we already added such a device (do this before setting timingSensorDevice)
+ ARMARX_DEBUG << "Adding the SensorDevice " << sd->getDeviceName() << " " << &sd ;
+ sensorDevices.add(sd->getDeviceName(), sd);
+ rtThreadTimingsSensorDevice = std::dynamic_pointer_cast<RTThreadTimingsSensorDevice>(sd);
+ }
+ else
+ {
+ ARMARX_DEBUG << "Adding the SensorDevice " << sd->getDeviceName() << " " << &sd ;
+ sensorDevices.add(sd->getDeviceName(), sd);
}
- //this checks if we already added such a device (do this before setting timingSensorDevice)
- ARMARX_DEBUG << "Adding the SensorDevice " << sd->getDeviceName() << " " << &sd ;
- sensorDevices.add(sd->getDeviceName(), sd);
- rtThreadTimingsSensorDevice = std::dynamic_pointer_cast<RTThreadTimingsSensorDevice>(sd);
- }
- else
- {
- ARMARX_DEBUG << "Adding the SensorDevice " << sd->getDeviceName() << " " << &sd ;
- sensorDevices.add(sd->getDeviceName(), sd);
}
ARMARX_INFO << "added SensorDevice " << sd->getDeviceName() << " (valuetype = " << sd->getSensorValueType() << ")";
+ throwIfStateIsNot(RobotUnitState::InitializingDevices, __FUNCTION__);
}
RTThreadTimingsSensorDevicePtr Devices::createRTThreadTimingSensorDevice() const
@@ -336,6 +390,8 @@ namespace armarx
void Devices::_postFinishRunning()
{
throwIfInControlThread(__FUNCTION__);
+ std::lock_guard<MutexType> guardS {sensorDevicesMutex};
+ std::lock_guard<MutexType> guardC {controlDevicesMutex};
controlDevicesConstPtr.clear();
sensorDevicesConstPtr.clear();
sensorDevices.clear();
@@ -346,7 +402,7 @@ namespace armarx
{
throwIfDevicesNotReady(__FUNCTION__);
throwIfInControlThread(__FUNCTION__);
-
+ std::lock_guard<MutexType> guard {controlDevicesMutex};
std::vector<JointController*> controllers;
controllers.reserve(controlDevices.values().size());
for (const ControlDevicePtr& dev : controlDevices.values())
@@ -356,6 +412,7 @@ namespace armarx
ARMARX_CHECK_NOT_NULL(controllers.back());
}
ARMARX_CHECK_EQUAL(controlDevices.size(), controllers.size());
+ throwIfDevicesNotReady(__FUNCTION__);
return controllers;
}
@@ -363,7 +420,7 @@ namespace armarx
{
throwIfDevicesNotReady(__FUNCTION__);
throwIfInControlThread(__FUNCTION__);
-
+ std::lock_guard<MutexType> guard {controlDevicesMutex};
std::vector<JointController*> controllers;
controllers.reserve(controlDevices.values().size());
for (const ControlDevicePtr& dev : controlDevices.values())
@@ -373,12 +430,15 @@ namespace armarx
ARMARX_CHECK_NOT_NULL(controllers.back());
}
ARMARX_CHECK_EQUAL(controlDevices.size(), controllers.size());
+ throwIfDevicesNotReady(__FUNCTION__);
return controllers;
}
void Devices::_preFinishDeviceInitialization()
{
throwIfInControlThread(__FUNCTION__);
+ std::lock_guard<MutexType> guardS {sensorDevicesMutex};
+ std::lock_guard<MutexType> guardC {controlDevicesMutex};
if (!sensorDevices.has(rtThreadTimingsSensorDeviceName))
{
addSensorDevice(createRTThreadTimingSensorDevice());
@@ -388,6 +448,8 @@ namespace armarx
void Devices::_postFinishDeviceInitialization()
{
throwIfInControlThread(__FUNCTION__);
+ std::lock_guard<MutexType> guardS {sensorDevicesMutex};
+ std::lock_guard<MutexType> guardC {controlDevicesMutex};
ARMARX_DEBUG << "checking " << controlDevices.size() << " ControlDevices:";
{
for (const ControlDevicePtr& controlDevice : controlDevices.values())
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.h b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.h
index 0872a2be7a9d1a6a5378baf6d4c237b347d3d945..b4f2368d55dc62f957a14c1b8fff0200f7d39e66 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.h
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.h
@@ -403,18 +403,24 @@ namespace armarx
std::vector<std::size_t> groupIndices;
};
private:
+ //ctrl dev
+ /// @brief \ref ControlDevice "ControlDevices" added to this unit (data may only be added during and only be used after \ref State::InitializingDevices)
+ KeyValueVector<std::string, ControlDevicePtr> controlDevices;
+ /// @brief const pointer to all ControlDevices (passed to GenerateConfigDescription of a NJointController)
+ std::map<std::string, ConstControlDevicePtr> controlDevicesConstPtr;
+ /// @brief Guards access to all \ref ControlDevice "ControlDevices"
+ mutable MutexType controlDevicesMutex;
+
/// @brief Device groups requiring the same hardware control mode.
ControlDeviceHardwareControlModeGroups ctrlModeGroups;
- /// @brief ControlDevices added to this unit (data may only be added during and only be used after State::InitializingDevices)
- KeyValueVector<std::string, ControlDevicePtr> controlDevices;
- /// @brief SensorDevices added to this unit (data may only be added during and only be used after State::InitializingDevices)
+ //sens dev
+ /// @brief \ref SensorDevice "SensorDevices" added to this unit (data may only be added during and only be used after \ref State::InitializingDevices)
KeyValueVector<std::string, SensorDevicePtr > sensorDevices;
-
- /// @brief const pointer to all ControlDevices (passed to GenerateConfigDescription of a NJointController)
- std::map<std::string, ConstControlDevicePtr> controlDevicesConstPtr;
/// @brief const pointer to all SensorDevices (passed to GenerateConfigDescription of a NJointController)
std::map<std::string, ConstSensorDevicePtr> sensorDevicesConstPtr;
+ /// @brief Guards access to all \ref SensorDevice "SensorDevices"
+ mutable MutexType sensorDevicesMutex;
/// @brief a pointer to the RTThreadTimingsSensorDevice used to meassure timings in the rt loop
RTThreadTimingsSensorDevicePtr rtThreadTimingsSensorDevice;